From 2e248e7bc0c682a49aa9cd0af47b4a7ea51ae62d Mon Sep 17 00:00:00 2001
From: Arne Schwabe <arne@rfc2549.org>
Date: Fri, 10 Nov 2017 16:41:52 +0100
Subject: [PATCH] Do double pythrogoras if altitude is involved (fixes #5280)
 (#5341)

---
 radio/src/telemetry/telemetry_sensors.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp
index 64544b90f..52c5ea02a 100644
--- a/radio/src/telemetry/telemetry_sensors.cpp
+++ b/radio/src/telemetry/telemetry_sensors.cpp
@@ -315,12 +315,16 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
         dist = gpsItem.distFromEarthAxis * angle / 1000000;
         result += dist*dist;
 
+        // Length on ground (ignoring curvature of the earth)
+        result = isqrt32(result);
+
         if (altItem) {
           dist = abs(altItem->value) / g_model.telemetrySensors[sensor.dist.alt-1].getPrecDivisor();
-          result += dist*dist;
+          result = dist*dist + result*result;
+          result = isqrt32(result);
         }
 
-        setValue(sensor, isqrt32(result), UNIT_METERS);
+        setValue(sensor, result, UNIT_METERS);
       }
       break;