diff --git a/radio/src/opentx.h b/radio/src/opentx.h
index 339e9ecda..742c35818 100644
--- a/radio/src/opentx.h
+++ b/radio/src/opentx.h
@@ -25,8 +25,10 @@
 #include <string.h>
 #include <stddef.h>
 #include <stdlib.h>
+#include <math.h>
 #include "definitions.h"
 #include "opentx_types.h"
+
 #if defined(STM32)
 #include "usbd_conf.h"
 #endif
@@ -1268,8 +1270,7 @@ enum TelemetryViews {
 extern uint8_t s_frsky_view;
 #endif
 
-#define EARTH_RADIUSKM ((uint32_t)6371)
-#define EARTH_RADIUS ((uint32_t)111194) // meters * pi / 180°
+constexpr uint32_t EARTH_RADIUS = 6371009;
 
 void getGpsPilotPosition();
 void getGpsDistance();
diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp
index 190d036a3..88489a9d3 100644
--- a/radio/src/telemetry/telemetry_sensors.cpp
+++ b/radio/src/telemetry/telemetry_sensors.cpp
@@ -72,9 +72,9 @@ bool isFaiForbidden(source_t idx)
 uint32_t getDistFromEarthAxis(int32_t latitude)
 {
   uint32_t lat = abs(latitude) / 10000;
-  uint32_t angle2 = (lat*lat) / 10000;
+  uint32_t angle2 = (lat * lat) / 10000;
   uint32_t angle4 = angle2 * angle2;
-  return 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
+  return 139*(((uint32_t)10000000 - ((angle2*(uint32_t)123370)/81) + (angle4/25))/12500);
 }
 
 void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32_t unit, uint32_t prec)
@@ -334,7 +334,7 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
     case TELEM_FORMULA_DIST:
       if (sensor.dist.gps) {
         TelemetryItem gpsItem = telemetryItems[sensor.dist.gps-1];
-        TelemetryItem * altItem = NULL;
+        TelemetryItem * altItem = nullptr;
         if (!gpsItem.isAvailable()) {
           return;
         }
@@ -353,19 +353,19 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
           }
         }
         uint32_t angle = abs(gpsItem.gps.latitude - gpsItem.pilotLatitude);
-        uint32_t dist = EARTH_RADIUS * angle / 1000000;
-        uint32_t result = dist*dist;
+        uint32_t dist = (EARTH_RADIUS * M_PI / 180) * angle / 1000000;
+        uint32_t result = dist * dist;
 
         angle = abs(gpsItem.gps.longitude - gpsItem.pilotLongitude);
         dist = gpsItem.distFromEarthAxis * angle / 1000000;
-        result += dist*dist;
+        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*result;
+          result = (dist * dist) + (result * result);
           result = isqrt32(result);
         }