//--------------------------------------------------------------------
//(GETATAN2) Simplyfied ArcTan2 function based on fixed point ArcCos
//ArcTanX - Input X
//ArcTanY - Input Y
//ArcTan4 - Output ARCTAN2(X/Y)
//XYhyp2 - Output presenting Hypotenuse of X and Y
s16 arctan2(s16 atanX, s16 atanY, long *hyp2XY)
{
s16 angleRad4;
s16 atan4;
long temp;
temp = isqrt32(((long)atanX*atanX*DEC_EXP_4) + ((long)atanY*atanY*DEC_EXP_4));
angleRad4 = arccos (((long)atanX*(long)DEC_EXP_6) /(long) temp);
if (atanY < 0) // removed overhead... Atan4 = angleRad4 * (atanY/abs(atanY));
atan4 = -angleRad4;
else
atan4 = angleRad4;
if (hyp2XY)
*hyp2XY = temp;
return atan4;
}
void getGpsDistance()
{
uint32_t lat, lng;
extractLatitudeLongitude(&lat, &lng);
// printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyData.hub.pilotLatitude), lng, abs(lng - frskyData.hub.pilotLongitude));
uint32_t angle = (lat > frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.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;
result += dist*dist;
dist = abs(frskyData.hub.baroAltitudeOffset ? TELEMETRY_ALT_BP : TELEMETRY_GPS_ALT_BP);
result += dist*dist;
frskyData.hub.gpsDistance = isqrt32(result);
if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance)
frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance;
}
