Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
28 #include "pprz_geodetic_double.h"
29 
30 #include <math.h>
31 #include "std.h" /* for RadOfDeg */
32 
33 
34 void 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 
57 void 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 */
83 void 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 
120 void 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 
140 void 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 
147 void 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);
151  ENU_OF_TO_NED(*ned, enu);
152 }
153 
154 void 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 
159 void 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);
163  ENU_OF_TO_NED(*ned, enu);
164 }
165 
166 
167 
168 void 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 
174 void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
175 {
176  struct EnuCoor_d enu;
177  ENU_OF_TO_NED(enu, *ned);
178  ecef_of_enu_point_d(ecef, def, &enu);
179 }
180 
181 void 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 
186 void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
187 {
188  struct EnuCoor_d enu;
189  ENU_OF_TO_NED(enu, *ned);
190  ecef_of_enu_vect_d(ecef, def, &enu);
191 }
192 
193 
194 void 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 
201 void 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 */
210 double 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 
221 #include "math/pprz_geodetic_utm.h"
222 
223 static 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 
228 static inline double isometric_latitude_fast_d(double phi)
229 {
230  return log(tan(M_PI_4 + phi / 2.0));
231 }
232 
233 static 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  */
276 void 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));
287  double ll_ = isometric_latitude_fast_d(phi_);
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  */
311 void 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
Definition: baro_MS5534A.c:203
static const float scale[]
double m[3 *3]
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
#define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:476
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:463
#define VECT2_SUB(_a, _b)
Definition: pprz_algebra.h:80
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
double x
in meters
struct EcefCoor_d ecef
origin of local frame in ECEF
uint8_t zone
UTM zone number.
double y
in meters
struct LlaCoor_d lla
origin of local frame in LLA
double z
in meters
double east
in meters
double lat
in radians
double north
in meters
double alt
in meters above WGS84 reference ellipsoid
struct DoubleRMat ltp_of_ecef
rotation from ECEF to local frame
double lon
in radians
double alt
in meters (above WGS84 reference ellipsoid or above MSL)
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)
Definition: pprz_geodetic.h:58
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:41
#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
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]
Definition: trilateration.c:31
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
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