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

Telemetry files split

Browse Source
This commit is contained in:
Bertrand Songis
2016-03-28 11:24:55 +02:00
parent 485d30f65b
commit 746dc450fa
25 changed files with 1713 additions and 1609 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/maths.cpp
+15 -15
View File
@@ -143,21 +143,21 @@ double gpsToDouble(bool neg, int16_t bp, int16_t ap)
#if defined(FRSKY_HUB) && !defined(CPUARM)
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
{
div_t qr = div(frskyData.hub.gpsLatitude_bp, 100);
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLatitude_ap) * 5) / 3;
div_t qr = div(telemetryData.hub.gpsLatitude_bp, 100);
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + telemetryData.hub.gpsLatitude_ap) * 5) / 3;
qr = div(frskyData.hub.gpsLongitude_bp, 100);
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3;
qr = div(telemetryData.hub.gpsLongitude_bp, 100);
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + telemetryData.hub.gpsLongitude_ap) * 5) / 3;
}
void getGpsPilotPosition()
{
extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude);
uint32_t lat = frskyData.hub.pilotLatitude / 10000;
extractLatitudeLongitude(&telemetryData.hub.pilotLatitude, &telemetryData.hub.pilotLongitude);
uint32_t lat = telemetryData.hub.pilotLatitude / 10000;
uint32_t angle2 = (lat*lat) / 10000;
uint32_t angle4 = angle2 * angle2;
frskyData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
// TRACE("frskyData.hub.distFromEarthAxis=%d", frskyData.hub.distFromEarthAxis);
telemetryData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
// TRACE("telemetryData.hub.distFromEarthAxis=%d", telemetryData.hub.distFromEarthAxis);
}
void getGpsDistance()
@@ -166,21 +166,21 @@ void getGpsDistance()
extractLatitudeLongitude(&lat, &lng);
// printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyData.hub.pilotLatitude), lng, abs(lng - frskyData.hub.pilotLongitude));
// printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - telemetryData.hub.pilotLatitude), lng, abs(lng - telemetryData.hub.pilotLongitude));
uint32_t angle = (lat > frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.hub.pilotLatitude - lat;
uint32_t angle = (lat > telemetryData.hub.pilotLatitude) ? lat - telemetryData.hub.pilotLatitude : telemetryData.hub.pilotLatitude - lat;
uint32_t dist = EARTH_RADIUS * angle / 1000000;
uint32_t result = dist*dist;
angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng;
dist = frskyData.hub.distFromEarthAxis * angle / 1000000;
angle = (lng > telemetryData.hub.pilotLongitude) ? lng - telemetryData.hub.pilotLongitude : telemetryData.hub.pilotLongitude - lng;
dist = telemetryData.hub.distFromEarthAxis * angle / 1000000;
result += dist*dist;
dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP);
result += dist*dist;
frskyData.hub.gpsDistance = isqrt32(result);
if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance)
frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance;
telemetryData.hub.gpsDistance = isqrt32(result);
if (telemetryData.hub.gpsDistance > telemetryData.hub.maxGpsDistance)
telemetryData.hub.maxGpsDistance = telemetryData.hub.gpsDistance;
}
#endif