From 4d557f95ea01149d7d35812969cb5471612ec449 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Sun, 8 Jan 2023 19:27:06 +1030
Subject: [PATCH] Copter: Use filtered and corrected range finder in surface
 tracking

---
 ArduCopter/surface_tracking.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp
index 63f48902ad..669e7c646a 100644
--- a/ArduCopter/surface_tracking.cpp
+++ b/ArduCopter/surface_tracking.cpp
@@ -17,7 +17,7 @@ void Copter::SurfaceTracking::update_surface_offset()
         // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m.  curr_surface_alt_above_origin_cm is 7m (or 700cm)
         RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
         const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
-        const float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_position_z_up_cm() - dir * rf_state.alt_cm;
+        const float curr_surface_alt_above_origin_cm = rf_state.inertial_alt_cm - dir * rf_state.alt_cm_filt.get();
 
         // update position controller target offset to the surface's alt above the EKF origin
         copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm);