Skip to content

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

Merged
mcroomp merged 1 commit into
mainfrom
fix/anchor-gps-baro-altitude
Jul 18, 2026
Merged

Fix SITL vertical anchor altitude bug; add EK3 baro/GPS altitude blending#12
mcroomp merged 1 commit into
mainfrom
fix/anchor-gps-baro-altitude

Conversation

@mcroomp

@mcroomp mcroomp commented Jul 18, 2026

Copy link
Copy Markdown
Owner

Summary

Fixes a SITL crash in test_lua_flight_steady_sitl:
rawes.lua:315: bad argument #2 to 'set_target_angle_and_rate_and_throttle' (out of range).

Root cause: ArduPilot's vertical position estimate is baro-anchored — whatever
altitude the vehicle physically occupies when the EKF's local origin is
established becomes its "zero" (real baro-sensor behavior, not a simulation
artifact). This SITL test boots the vehicle already kinematically locked
ctx.home_alt_m metres above the anchor, so the anchor's sent altitude
(RAWES_AAL, via NAMED_VALUE_INT) needs to be offset by -ctx.home_alt_m to
resolve to the correct (positive-down) D offset once EKF-local. Without this
offset, the elevation-angle computation (asin(-rz/tlen)) was drastically
wrong, driving the cyclic pitch command to saturate until it exceeded
ArduPilot's valid range.

Diagnosed by comparing telemetry.csv's ground-truth pos_z (constant, correct)
against ekf_pos_z (stuck near 0 throughout) — confirming ArduPilot's own
vertical EKF estimate never reflected the true altitude.

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. Also includes general cleanup (unused numpy import removed, a
    stale log message corrected, minor blank-line/indentation fixes).
  • simulation/scripts/rawes.lua: adds throttled (~1 Hz) pre-capture
    diagnostic logging of pos_ned/anchor geometry, and expands the "captured"
    STATUSTEXT with explicit geometry values — a permanent diagnostic tool for
    future geometry-capture bugs.
  • simulation/tests/sitl/rawes_common_defaults.parm: sets
    EK3_OGN_HGT_MASK=5 (bits 0+2: correct when Baro is the active height
    source, apply the correction to local position) so GPS altitude
    progressively corrects baro-drift bias in the EKF's local vertical position
    over long real-world flights — mitigates weather-driven QNH drift, which
    the one-time-resolved anchor altitude can't otherwise self-correct.
  • Remaining files (gcs.py, mediator.py, mock_ardupilot.lua,
    rawes_lua_harness.py, rawes_modes.py, stack_infra.py, stack_utils.py,
    analysis/*.py, various simtests/unit tests, design docs, AGENTS.md)
    carry the broader anchor-location-over-GPS migration this fix depends on:
    transmitting the tether anchor's absolute GPS location
    (RAWES_LAT/RAWES_LON/RAWES_AAL via NAMED_VALUE_INT) instead of
    assuming a fixed NED offset, so the design works with arbitrary real-world
    GPS coordinates rather than only a fixed simulated origin.

Validation

bash test.sh stack -n 1 -k test_lua_flight_steady_sitlPASSED
(stable=128s, max_activity=423 PWM), confirmed both before and after adding
EK3_OGN_HGT_MASK=5.

Pumping and landing SITL stack tests have not yet been re-validated with these
changes (next step per AGENTS.md).

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.
@mcroomp
mcroomp merged commit 4c41eee into main Jul 18, 2026
10 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant