piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Just some comments, the very minimum

Browse Source
This commit is contained in:
Bertrand Songis
2017-05-07 22:27:10 +02:00
parent 301e33a271
commit 93dbdf8680
2 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/maths.cpp
+4 -1
View File
@@ -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);
}