Fix SITL vertical anchor altitude bug; add EK3 baro/GPS altitude blending#12
Merged
Conversation
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.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
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_mmetres above the anchor, so the anchor's sent altitude(
RAWES_AAL, via NAMED_VALUE_INT) needs to be offset by-ctx.home_alt_mtoresolve to the correct (positive-down) D offset once EKF-local. Without this
offset, the elevation-angle computation (
asin(-rz/tlen)) was drasticallywrong, 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 ownvertical 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_minstead of the raw homealtitude. Also includes general cleanup (unused
numpyimport removed, astale log message corrected, minor blank-line/indentation fixes).
simulation/scripts/rawes.lua: adds throttled (~1 Hz) pre-capturediagnostic 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: setsEK3_OGN_HGT_MASK=5(bits 0+2: correct when Baro is the active heightsource, 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.
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_AALvia NAMED_VALUE_INT) instead ofassuming 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_sitl→ PASSED(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).