43 const float sin_lat = sinf(def->
lla.
lat);
44 const float cos_lat = cosf(def->
lla.
lat);
45 const float sin_lon = sinf(def->
lla.
lon);
46 const float cos_lon = cosf(def->
lla.
lon);
68 const float sin_lat = sinf(def->
lla.
lat);
69 const float cos_lat = cosf(def->
lla.
lat);
70 const float sin_lon = sinf(def->
lla.
lon);
71 const float cos_lon = cosf(def->
lla.
lon);
143 enu_d.
x = (
double) enu->
x;
144 enu_d.y = (double) enu->
y;
145 enu_d.z = (
double) enu->
z;
152 ecef->
x = (float) ecef_d.
x + def->
ecef.
x;
153 ecef->
y = (
float) ecef_d.
y + def->
ecef.
y;
154 ecef->
z = (float) ecef_d.
z + def->
ecef.
z;
178 enu_d.
x = (
double) enu->
x;
179 enu_d.y = (double) enu->
y;
180 enu_d.z = (
double) enu->
z;
187 ecef->
x = (float) ecef_d.
x;
188 ecef->
y = (
float) ecef_d.
y;
189 ecef->
z = (float) ecef_d.
z;
208 static const float a = 6378137.0;
209 static const float f = 1. / 298.257223563;
210 const float b = a * (1. - f);
211 const float b2 = b * b;
213 const float e2 = 2.*f - (f * f);
214 const float ep2 = f * (2. - f) / ((1. - f) * (1. - f));
215 const float E2 = a * a - b2;
218 const float z2 = in->
z * in->
z;
219 const float r2 = in->
x * in->
x + in->
y * in->
y;
220 const float r = sqrtf(r2);
221 const float F = 54.*b2 *
z2;
222 const float G = r2 + (1 - e2) * z2 - e2 * E2;
223 const float c = (e2 * e2 * F * r2) / (G * G * G);
224 const float s = powf((1 + c + sqrtf(c * c + 2 * c)), 1. / 3.);
225 const float s1 = 1 + s + 1 / s;
226 const float P = F / (3 * s1 * s1 * G * G);
227 const float Q = sqrtf(1 + 2 * e2 * e2 * P);
228 const float ro = -(e2 * P * r) / (1 + Q) + sqrtf((a * a / 2) * (1 + 1 / Q) - ((1 - e2) * P * z2) / (Q *
229 (1 + Q)) - P * r2 / 2);
230 const float tmp = (r - e2 * ro) * (r - e2 * ro);
231 const float U = sqrtf(tmp + z2);
232 const float V = sqrtf(tmp + (1 - e2) * z2);
233 const float zo = (b2 * in->
z) / (a * V);
235 out->
alt = U * (1 - b2 / (a * V));
236 out->
lat = atanf((in->
z + ep2 * zo) / r);
237 out->
lon = atan2f(in->
y, in->
x);
245 static const float a = 6378137.0;
246 static const float f = 1. / 298.257223563;
247 const float e2 = 2.*f - (f * f);
249 const float sin_lat = sinf(in->
lat);
250 const float cos_lat = cosf(in->
lat);
251 const float sin_lon = sinf(in->
lon);
252 const float cos_lon = cosf(in->
lon);
253 const float chi = sqrtf(1. - e2 * sin_lat * sin_lat);
254 const float a_chi = a / chi;
256 out->
x = (a_chi + in->
alt) * cos_lat * cos_lon;
257 out->
y = (a_chi + in->
alt) * cos_lat * sin_lon;
258 out->
z = (a_chi * (1. - e2) + in->
alt) * sin_lat;
267 #define CScal(k, z) { z.re *= k; z.im *= k; }
268 #define CAdd(z1, z2) { z2.re += z1.re; z2.im += z1.im; }
269 #define CSub(z1, z2) { z2.re -= z1.re; z2.im -= z1.im; }
270 #define CI(z) { float tmp = z.re; z.re = - z.im; z.im = tmp; }
271 #define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); }
274 #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cosf(z.im); z.re = e*cos_z_im; float sin_z_im = sinf(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); }
279 return logf(tanf(M_PI_4 + phi / 2.0)) - e / 2.0 * logf((1.0 + e * sinf(phi)) / (1.0 - e * sinf(phi)));
284 return logf(tanf(M_PI_4 + phi / 2.0));
289 float exp_l = expf(lat);
290 float phi0 = 2 * atanf(exp_l) - M_PI_2;
296 float sin_phi = e * sinf(phi_);
297 phi0 = 2 * atanf(powf((1 + sin_phi) / (1. - sin_phi), e / 2.) * exp_l) - M_PI_2;
299 }
while (max_iter && fabsf(phi_ - phi0) > epsilon);
311 if (utm->
zone == 0) {
317 float dl = lla->
lon - lambda_c;
318 float phi_ = asinf(sinf(dl) / coshf(ll));
320 float lambda_ = atanf(sinhf(ll) / cosf(dl));
321 struct complex z_ = { lambda_, ll_ };
324 for (k = 1; k < 3; k++) {
325 struct complex z = { lambda_, ll_ };
349 struct complex z = { real, img };
352 for (k = 1; k < 2; k++) {
353 struct complex z_ = { real, img };
361 lla->
lon = lambda_c + atanf(sinhf(z.
im) / cosf(z.
re));
362 float phi_ = asinf(sinf(z.
re) / coshf(z.
im));
void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
definition of the local (flat earth) coordinate system
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
vector in EarthCenteredEarthFixed coordinates
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
vector in East North Up coordinates Units: meters
#define VECT3_COPY(_a, _b)
#define VECT3_DIFF(_c, _a, _b)
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
position in UTM coordinates Units: meters
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef)
static float inverse_isometric_latitude_f(float lat, float e, float epsilon)
#define ENU_OF_TO_NED(_po, _pi)
Paparazzi double-precision floating point math for geodetic calculations.
struct EcefCoor_f ecef
origin of local frame in ECEF
Paparazzi floating point math for geodetic calculations.
vector in Latitude, Longitude and Altitude
#define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin)
static float isometric_latitude_fast_f(float phi)
vector in North East Down coordinates Units: meters
Paparazzi floating point algebra.
static const float serie_coeff_proj_mercator_inverse[5]
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
uint8_t zone
UTM zone number.
vector in EarthCenteredEarthFixed coordinates
#define LLA_COPY(_pos1, _pos2)
struct FloatRMat ltp_of_ecef
rotation from ECEF to local frame
float alt
in meters (normally above WGS84 reference ellipsoid)
void ltp_def_from_lla_f(struct LtpDef_f *def, struct LlaCoor_f *lla)
Constants UTM (Mercator) projections.
static const float scale[]
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
#define LambdaOfUtmZone(utm_zone)
static const float serie_coeff_proj_mercator[5]
static float isometric_latitude_f(float phi, float e)
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
struct LlaCoor_f lla
origin of local frame in LLA
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
void ecef_of_enu_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
#define UtmZoneOfLlaLonRad(lla_lon)