Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
pprz_geodetic_double.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2014 The Paparazzi Team
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
29
30#include <math.h>
31#include "std.h" /* for RadOfDeg */
32
33
34void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
35{
36 /* store the origin of the tangent plane */
37 VECT3_COPY(def->ecef, *ecef);
38 /* compute the lla representation of the origin */
39 lla_of_ecef_d(&def->lla, &def->ecef);
40 /* store the rotation matrix */
41 const double sin_lat = sin(def->lla.lat);
42 const double cos_lat = cos(def->lla.lat);
43 const double sin_lon = sin(def->lla.lon);
44 const double cos_lon = cos(def->lla.lon);
45 def->ltp_of_ecef.m[0] = -sin_lon;
46 def->ltp_of_ecef.m[1] = cos_lon;
47 def->ltp_of_ecef.m[2] = 0.;
48 def->ltp_of_ecef.m[3] = -sin_lat * cos_lon;
49 def->ltp_of_ecef.m[4] = -sin_lat * sin_lon;
50 def->ltp_of_ecef.m[5] = cos_lat;
51 def->ltp_of_ecef.m[6] = cos_lat * cos_lon;
52 def->ltp_of_ecef.m[7] = cos_lat * sin_lon;
53 def->ltp_of_ecef.m[8] = sin_lat;
54
55}
56
57void ltp_def_from_lla_d(struct LtpDef_d *def, struct LlaCoor_d *lla)
58{
59 /* store the origin of the tangent plane */
60 LLA_COPY(def->lla, *lla);
61 /* compute the ecef representation of the origin */
62 ecef_of_lla_d(&def->ecef, &def->lla);
63
64 /* store the rotation matrix */
65 const double sin_lat = sin(def->lla.lat);
66 const double cos_lat = cos(def->lla.lat);
67 const double sin_lon = sin(def->lla.lon);
68 const double cos_lon = cos(def->lla.lon);
69
70 def->ltp_of_ecef.m[0] = -sin_lon;
71 def->ltp_of_ecef.m[1] = cos_lon;
72 /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
73 def->ltp_of_ecef.m[2] = 0.;
74 def->ltp_of_ecef.m[3] = -sin_lat * cos_lon;
75 def->ltp_of_ecef.m[4] = -sin_lat * sin_lon;
76 def->ltp_of_ecef.m[5] = cos_lat;
77 def->ltp_of_ecef.m[6] = cos_lat * cos_lon;
78 def->ltp_of_ecef.m[7] = cos_lat * sin_lon;
79 def->ltp_of_ecef.m[8] = sin_lat;
80}
81
82/* http://en.wikipedia.org/wiki/Geodetic_system */
83void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
84{
85
86 // FIXME : make an ellipsoid struct
87 static const double a = 6378137.0; /* earth semimajor axis in meters */
88 static const double f = 1. / 298.257223563; /* reciprocal flattening */
89 const double b = a * (1. - f); /* semi-minor axis */
90 const double b2 = b * b;
91
92 const double e2 = 2.*f - (f * f); /* first eccentricity squared */
93 const double ep2 = f * (2. - f) / ((1. - f) * (1. - f)); /* second eccentricity squared */
94 const double E2 = a * a - b2;
95
96
97 const double z2 = ecef->z * ecef->z;
98 const double r2 = ecef->x * ecef->x + ecef->y * ecef->y;
99 const double r = sqrt(r2);
100 const double F = 54.*b2 * z2;
101 const double G = r2 + (1 - e2) * z2 - e2 * E2;
102 const double c = (e2 * e2 * F * r2) / (G * G * G);
103 const double s = pow((1 + c + sqrt(c * c + 2 * c)), 1. / 3.);
104 const double s1 = 1 + s + 1 / s;
105 const double P = F / (3 * s1 * s1 * G * G);
106 const double Q = sqrt(1 + 2 * e2 * e2 * P);
107 const double ro = -(e2 * P * r) / (1 + Q) + sqrt((a * a / 2) * (1 + 1 / Q) - ((1 - e2) * P * z2) / (Q *
108 (1 + Q)) - P * r2 / 2);
109 const double tmp = (r - e2 * ro) * (r - e2 * ro);
110 const double U = sqrt(tmp + z2);
111 const double V = sqrt(tmp + (1 - e2) * z2);
112 const double zo = (b2 * ecef->z) / (a * V);
113
114 lla->alt = U * (1 - b2 / (a * V));
115 lla->lat = atan((ecef->z + ep2 * zo) / r);
116 lla->lon = atan2(ecef->y, ecef->x);
117
118}
119
120void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
121{
122
123 // FIXME : make an ellipsoid struct
124 static const double a = 6378137.0; /* earth semimajor axis in meters */
125 static const double f = 1. / 298.257223563; /* reciprocal flattening */
126 const double e2 = 2.*f - (f * f); /* first eccentricity squared */
127
128 const double sin_lat = sin(lla->lat);
129 const double cos_lat = cos(lla->lat);
130 const double sin_lon = sin(lla->lon);
131 const double cos_lon = cos(lla->lon);
132 const double chi = sqrt(1. - e2 * sin_lat * sin_lat);
133 const double a_chi = a / chi;
134
135 ecef->x = (a_chi + lla->alt) * cos_lat * cos_lon;
136 ecef->y = (a_chi + lla->alt) * cos_lat * sin_lon;
137 ecef->z = (a_chi * (1. - e2) + lla->alt) * sin_lat;
138}
139
140void enu_of_ecef_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
141{
142 struct EcefCoor_d delta;
143 VECT3_DIFF(delta, *ecef, def->ecef);
144 MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta);
145}
146
147void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
148{
149 struct EnuCoor_d enu;
150 enu_of_ecef_point_d(&enu, def, ecef);
152}
153
154void enu_of_ecef_vect_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
155{
156 MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, *ecef);
157}
158
159void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
160{
161 struct EnuCoor_d enu;
162 enu_of_ecef_vect_d(&enu, def, ecef);
164}
165
166
167
168void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
169{
170 MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
171 VECT3_ADD(*ecef, def->ecef);
172}
173
174void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
175{
176 struct EnuCoor_d enu;
178 ecef_of_enu_point_d(ecef, def, &enu);
179}
180
181void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
182{
183 MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
184}
185
186void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
187{
188 struct EnuCoor_d enu;
190 ecef_of_enu_vect_d(ecef, def, &enu);
191}
192
193
194void enu_of_lla_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct LlaCoor_d *lla)
195{
196 struct EcefCoor_d ecef;
197 ecef_of_lla_d(&ecef, lla);
198 enu_of_ecef_point_d(enu, def, &ecef);
199}
200
201void ned_of_lla_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct LlaCoor_d *lla)
202{
203 struct EcefCoor_d ecef;
204 ecef_of_lla_d(&ecef, lla);
205 ned_of_ecef_point_d(ned, def, &ecef);
206}
207
208
209/* geocentric latitude of geodetic latitude */
210double gc_of_gd_lat_d(double gd_lat, double hmsl)
211{
212 const double a = 6378137.0; /* earth semimajor axis in meters */
213 const double f = 1. / 298.257223563; /* reciprocal flattening */
214 const double c2 = (1. - f) * (1. - f);
215 /* geocentric latitude at the planet surface */
216 double ls = atan(c2 * tan(gd_lat));
217 return atan2(hmsl * sin(gd_lat) + a * sin(ls), hmsl * cos(gd_lat) + a * cos(ls));
218}
219
220
222
223static inline double isometric_latitude_d(double phi, double e)
224{
225 return log(tan(M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
226}
227
228static inline double isometric_latitude_fast_d(double phi)
229{
230 return log(tan(M_PI_4 + phi / 2.0));
231}
232
233static inline double inverse_isometric_latitude_d(double lat, double e, double epsilon)
234{
235 double exp_l = exp(lat);
236 double phi0 = 2 * atan(exp_l) - M_PI_2;
237 double phi_;
238 uint8_t max_iter = 3; /* To be sure to return */
239
240 do {
241 phi_ = phi0;
242 double sin_phi = e * sin(phi_);
243 phi0 = 2 * atan(pow((1 + sin_phi) / (1. - sin_phi), e / 2.) * exp_l) - M_PI_2;
244 max_iter--;
245 } while (max_iter && fabs(phi_ - phi0) > epsilon);
246
247 return phi0;
248}
249
250#define CI(v) { \
251 double tmp = v.x; \
252 v.x = -v.y; \
253 v.y = tmp; \
254 }
255
256#define CExp(v) { \
257 double e = exp(v.x);\
258 v.x = e*cos(v.y); \
259 v.y = e*sin(v.y); \
260 }
261
262#define CSin(v) { \
263 CI(v); \
264 struct DoubleVect2 vstar = {-v.x, -v.y}; \
265 CExp(v); \
266 CExp(vstar); \
267 VECT2_SUB(v, vstar);\
268 VECT2_SMUL(v, v, -0.5); \
269 CI(v); \
270 }
271
272/* Convert lla to utm (double).
273 * @param[out] utm position in m, alt is copied directly from lla
274 * @param[in] lla position in rad, alt in m
275 */
276void utm_of_lla_d(struct UtmCoor_d *utm, struct LlaCoor_d *lla)
277{
278 // compute zone if not initialised
279 if (utm->zone == 0) {
280 utm->zone = UtmZoneOfLlaLonRad(lla->lon);
281 }
282
283 double lambda_c = LambdaOfUtmZone(utm->zone);
284 double ll = isometric_latitude_d(lla->lat , E);
285 double dl = lla->lon - lambda_c;
286 double phi_ = asin(sin(dl) / cosh(ll));
288 double lambda_ = atan(sinh(ll) / cos(dl));
289 struct DoubleVect2 z_ = { lambda_, ll_ };
291 int8_t k;
292 for (k = 1; k < 3; k++) {
293 struct DoubleVect2 z = { lambda_, ll_ };
294 VECT2_SMUL(z, z, 2.*k);
295 CSin(z);
297 VECT2_ADD(z_, z);
298 }
299 VECT2_SMUL(z_, z_, N);
300 utm->east = DELTA_EAST + z_.y;
301 utm->north = DELTA_NORTH + z_.x;
302
303 // copy alt above reference ellipsoid
304 utm->alt = lla->alt;
305}
306
307/* Convert utm to lla (double).
308 * @param[out] lla position in rad, alt is copied directly from utm
309 * @param[in] utm position in m, alt in m
310 */
311void lla_of_utm_d(struct LlaCoor_d *lla, struct UtmCoor_d *utm)
312{
313 struct DoubleVect2 z = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST};
314 double scale = 1 / N / serie_coeff_proj_mercator[0];
315 VECT2_SMUL(z, z, scale);
316
317 int8_t k;
318 for(k = 1; k<2; k++)
319 {
320 struct DoubleVect2 z_;
321 VECT2_SMUL(z_, z, 2.*k);
322 CSin(z_);
324 VECT2_SUB(z, z_);
325 }
326
327 double lambda_c = LambdaOfUtmZone(utm->zone);
328 lla->lon = lambda_c + atan(sinh(z.y) / cos(z.x));
329 double phi = asin(sin(z.x) / cosh(z.y));
330 double il = isometric_latitude_fast_d(phi);
331 lla->lat = inverse_isometric_latitude_d(il, E, 1e-8);
332
333 // copy alt above reference ellipsoid
334 lla->alt = utm->alt;
335}
static uint16_t c2
static const float scale[]
#define VECT2_ADD(_a, _b)
#define VECT2_SMUL(_vo, _vi, _s)
#define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin)
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
#define VECT2_SUB(_a, _b)
#define VECT3_COPY(_a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_ADD(_a, _b)
double x
in meters
double y
in meters
double z
in meters
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
double lon
in radians
void lla_of_utm_d(struct LlaCoor_d *lla, struct UtmCoor_d *utm)
double gc_of_gd_lat_d(double gd_lat, double hmsl)
void ned_of_lla_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct LlaCoor_d *lla)
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
void enu_of_ecef_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void enu_of_lla_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct LlaCoor_d *lla)
void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
void enu_of_ecef_vect_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
void utm_of_lla_d(struct UtmCoor_d *utm, struct LlaCoor_d *lla)
void ltp_def_from_lla_d(struct LtpDef_d *def, struct LlaCoor_d *lla)
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
#define LLA_COPY(_pos1, _pos2)
#define ENU_OF_TO_NED(_po, _pi)
#define N
#define E
static const float serie_coeff_proj_mercator[5]
static const float serie_coeff_proj_mercator_inverse[5]
#define UtmZoneOfLlaLonRad(lla_lon)
#define DELTA_EAST
#define DELTA_NORTH
#define LambdaOfUtmZone(utm_zone)
static uint32_t s
uint16_t foo
Definition main_demo5.c:58
float epsilon
static double isometric_latitude_d(double phi, double e)
static double isometric_latitude_fast_d(double phi)
static double inverse_isometric_latitude_d(double lat, double e, double epsilon)
#define CSin(v)
Paparazzi double-precision floating point math for geodetic calculations.
Constants UTM (Mercator) projections.
static float P[3][3]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition wedgebug.c:204
float b
Definition wedgebug.c:202