Plane: fix rangefinder correction when terrain follow is off

jschall · May 20, 2024 · d4e6c6f · d4e6c6f
1 parent 3e868be
commit d4e6c6f
Showing 1 changed file with 2 additions and 3 deletions.
diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp
@@ -711,9 +711,8 @@ void Plane::rangefinder_height_update(void)
     }
 
     if (rangefinder_state.in_range) {
-        // base correction is the difference between baro altitude and
-        // rangefinder estimate
-        float correction = adjusted_relative_altitude_cm()*0.01 - rangefinder_state.height_estimate;
+        // If not using terrain data, we expect zero correction when our height above target is equal to our rangefinder measurement
+        float correction = height_above_target() - rangefinder_state.height_estimate;
 
 #if AP_TERRAIN_AVAILABLE
         // if we are terrain following then correction is based on terrain data