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

Port of GPS modifications from SVN

Browse Source
This commit is contained in:
bsongis
2014-02-25 16:57:01 +01:00
parent c98e718e16
commit 590a58a6e9
8 changed files with 166 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/maths.cpp
+1 -22
View File
@@ -48,18 +48,6 @@ lcdint_t applyChannelRatio(uint8_t channel, lcdint_t val)
}
#endif
#if defined(PCBTARANIS)
double gpsToDouble(bool neg, int16_t bp, int16_t ap)
{
double result = ap;
result /= 10000;
result += (bp % 100);
result /= 60;
result += (bp / 100);
return neg?-result:result;
}
#endif
#if defined(FRSKY_HUB)
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
{
@@ -70,17 +58,8 @@ void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3;
}
#if defined(PCBTARANIS)
double pilotLatitude;
double pilotLongitude;
#endif
void getGpsPilotPosition()
{
#if defined(PCBTARANIS)
pilotLatitude = gpsToDouble(frskyData.hub.gpsLatitudeNS=='S', frskyData.hub.gpsLatitude_bp, frskyData.hub.gpsLatitude_ap);
pilotLongitude = gpsToDouble(frskyData.hub.gpsLongitudeEW=='W', frskyData.hub.gpsLongitude_bp, frskyData.hub.gpsLongitude_ap);
#endif
extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude);
uint32_t lat = frskyData.hub.pilotLatitude / 10000;
uint32_t angle2 = (lat*lat) / 10000;
@@ -105,7 +84,7 @@ void getGpsDistance()
dist = frskyData.hub.distFromEarthAxis * angle / 1000000;
result += dist*dist;
dist = abs(frskyData.hub.baroAltitudeOffset ? TELEMETRY_ALT_BP : TELEMETRY_GPS_ALT_BP);
dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP);
result += dist*dist;
frskyData.hub.gpsDistance = isqrt32(result);