mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Andy Piper
09c48d855f
terrain_srtm_alt is the terrain height above the EKF origin, positive up: AP_AHRS::writeTerrainAMSL() converts the AMSL height AP_Terrain supplies with alt_amsl_m - origin.alt, and the core stores it verbatim. pd is the vehicle's position.z, positive down. Height above ground is therefore (-pd) minus the terrain height, not the terrain height minus pd. The neighbouring terrainState expression is right because terrainState is itself a D coordinate, built as position.z + rngOnGnd, so the two branches of one variable were being differenced in opposite conventions. Where the AHRS and core origins coincide the old error is 2 x terrain_srtm_alt, so it is smallest where the origin sits at field elevation and grows with relief - and it moves the wrong way, reading high over ground that is above the origin. AP_AHRS subtracts the one public origin and hands the same figure to every core, which then differences it against its own position.z, so where a core's own origin altitude has moved - ekfGpsRefHgt drift, or lanes aligning against different receivers under EK3_AFFINITY - the corrected form still carries that difference. The trace here is complete only for the default EK3_OGN_HGT_MASK. A terrain height that disagrees with the datum still collapses the scale height to the on-ground range through the MAX, exactly as it does today. The old expression could go negative too - over ground that sits further below the origin than the vehicle sits above it - so this is pre-existing, and the sign fix moves which geometry triggers it rather than closing it. Falling back to terrainState there is the obvious repair and it does not work: the enclosing condition has already declared that state stale. Measured in SITL off the Kalaupapa cliffs, holding 60 m above an origin the ground falls to 160 m below, so the true height above ground reaches 220 m. GPS navigates, so flow is not fused into velocity and both builds fly the same trajectory; only the scale height differs. Across the traverse the logged flow innovation consistency ratio saturates its 255 ceiling - at least 2.55, so flow at that geometry is rejected outright - against a peak of 0 with this change. The raw innovation is deliberately not quoted: XKF5 logs it as an int16 scaled by 1000, and the old expression drives it past the wrap point, so the logged figure there is an aliased value rather than the real one.