diff --git a/radio/src/maths.cpp b/radio/src/maths.cpp index 01d228cb1..f1244b1af 100644 --- a/radio/src/maths.cpp +++ b/radio/src/maths.cpp @@ -128,6 +128,7 @@ uint16_t isqrt32(uint32_t n) */ #if defined(FRSKY_HUB) && !defined(CPUARM) +// convert latitude and longitude to 1/10^6 degrees void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) { div_t qr = div(telemetryData.hub.gpsLatitude_bp, 100); @@ -140,10 +141,12 @@ void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) void getGpsPilotPosition() { extractLatitudeLongitude(&telemetryData.hub.pilotLatitude, &telemetryData.hub.pilotLongitude); + // distFromEarthAxis = cos(lat) * EARTH_RADIUS + // 1 - x2/2 + x4/24 uint32_t lat = telemetryData.hub.pilotLatitude / 10000; uint32_t angle2 = (lat*lat) / 10000; uint32_t angle4 = angle2 * angle2; - telemetryData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); + telemetryData.hub.distFromEarthAxis = 139 * (((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25)) / 12500); // TRACE("telemetryData.hub.distFromEarthAxis=%d", telemetryData.hub.distFromEarthAxis); } diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 89e099521..7357971f0 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -1557,7 +1557,7 @@ extern uint8_t s_frsky_view; #endif #define EARTH_RADIUSKM ((uint32_t)6371) -#define EARTH_RADIUS ((uint32_t)111194) +#define EARTH_RADIUS ((uint32_t)111194) // meters * pi / 180° void getGpsPilotPosition(); void getGpsDistance();