Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
37  /* store the origin of the tangeant plane */
38  VECT3_COPY(def->ecef, *ecef);
39  /* compute the lla representation of the origin */
40  lla_of_ecef_d(&def->lla, &def->ecef);
41  /* store the rotation matrix */
42  const double sin_lat = sin(def->lla.lat);
43  const double cos_lat = cos(def->lla.lat);
44  const double sin_lon = sin(def->lla.lon);
45  const double cos_lon = cos(def->lla.lon);
46  def->ltp_of_ecef.m[0] = -sin_lon;
47  def->ltp_of_ecef.m[1] = cos_lon;
48  def->ltp_of_ecef.m[2] = 0.;
49  def->ltp_of_ecef.m[3] = -sin_lat * cos_lon;
50  def->ltp_of_ecef.m[4] = -sin_lat * sin_lon;
51  def->ltp_of_ecef.m[5] = cos_lat;
52  def->ltp_of_ecef.m[6] = cos_lat * cos_lon;
53  def->ltp_of_ecef.m[7] = cos_lat * sin_lon;
54  def->ltp_of_ecef.m[8] = sin_lat;
55 
56 }
57 
58 /* http://en.wikipedia.org/wiki/Geodetic_system */
59 void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
60 {
61 
62  // FIXME : make an ellipsoid struct
63  static const double a = 6378137.0; /* earth semimajor axis in meters */
64  static const double f = 1. / 298.257223563; /* reciprocal flattening */
65  const double b = a * (1. - f); /* semi-minor axis */
66  const double b2 = b * b;
67 
68  const double e2 = 2.*f - (f * f); /* first eccentricity squared */
69  const double ep2 = f * (2. - f) / ((1. - f) * (1. - f)); /* second eccentricity squared */
70  const double E2 = a * a - b2;
71 
72 
73  const double z2 = ecef->z * ecef->z;
74  const double r2 = ecef->x * ecef->x + ecef->y * ecef->y;
75  const double r = sqrt(r2);
76  const double F = 54.*b2 * z2;
77  const double G = r2 + (1 - e2) * z2 - e2 * E2;
78  const double c = (e2 * e2 * F * r2) / (G * G * G);
79  const double s = pow((1 + c + sqrt(c * c + 2 * c)), 1. / 3.);
80  const double s1 = 1 + s + 1 / s;
81  const double P = F / (3 * s1 * s1 * G * G);
82  const double Q = sqrt(1 + 2 * e2 * e2 * P);
83  const double ro = -(e2 * P * r) / (1 + Q) + sqrt((a * a / 2) * (1 + 1 / Q) - ((1 - e2) * P * z2) / (Q *
84  (1 + Q)) - P * r2 / 2);
85  const double tmp = (r - e2 * ro) * (r - e2 * ro);
86  const double U = sqrt(tmp + z2);
87  const double V = sqrt(tmp + (1 - e2) * z2);
88  const double zo = (b2 * ecef->z) / (a * V);
89 
90  lla->alt = U * (1 - b2 / (a * V));
91  lla->lat = atan((ecef->z + ep2 * zo) / r);
92  lla->lon = atan2(ecef->y, ecef->x);
93 
94 }
95 
96 void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
97 {
98 
99  // FIXME : make an ellipsoid struct
100  static const double a = 6378137.0; /* earth semimajor axis in meters */
101  static const double f = 1. / 298.257223563; /* reciprocal flattening */
102  const double e2 = 2.*f - (f * f); /* first eccentricity squared */
103 
104  const double sin_lat = sin(lla->lat);
105  const double cos_lat = cos(lla->lat);
106  const double sin_lon = sin(lla->lon);
107  const double cos_lon = cos(lla->lon);
108  const double chi = sqrt(1. - e2 * sin_lat * sin_lat);
109  const double a_chi = a / chi;
110 
111  ecef->x = (a_chi + lla->alt) * cos_lat * cos_lon;
112  ecef->y = (a_chi + lla->alt) * cos_lat * sin_lon;
113  ecef->z = (a_chi * (1. - e2) + lla->alt) * sin_lat;
114 }
115 
116 void enu_of_ecef_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
117 {
118  struct EcefCoor_d delta;
119  VECT3_DIFF(delta, *ecef, def->ecef);
120  MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta);
121 }
122 
123 void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
124 {
125  struct EnuCoor_d enu;
126  enu_of_ecef_point_d(&enu, def, ecef);
127  ENU_OF_TO_NED(*ned, enu);
128 }
129 
130 void enu_of_ecef_vect_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
131 {
132  MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, *ecef);
133 }
134 
135 void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
136 {
137  struct EnuCoor_d enu;
138  enu_of_ecef_vect_d(&enu, def, ecef);
139  ENU_OF_TO_NED(*ned, enu);
140 }
141 
142 
143 
144 void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
145 {
146  MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
147  VECT3_ADD(*ecef, def->ecef);
148 }
149 
150 void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
151 {
152  struct EnuCoor_d enu;
153  ENU_OF_TO_NED(enu, *ned);
154  ecef_of_enu_point_d(ecef, def, &enu);
155 }
156 
157 void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
158 {
159  MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
160 }
161 
162 void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
163 {
164  struct EnuCoor_d enu;
165  ENU_OF_TO_NED(enu, *ned);
166  ecef_of_enu_vect_d(ecef, def, &enu);
167 }
168 
169 
170 void enu_of_lla_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct LlaCoor_d *lla)
171 {
172  struct EcefCoor_d ecef;
173  ecef_of_lla_d(&ecef, lla);
174  enu_of_ecef_point_d(enu, def, &ecef);
175 }
176 
177 void ned_of_lla_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct LlaCoor_d *lla)
178 {
179  struct EcefCoor_d ecef;
180  ecef_of_lla_d(&ecef, lla);
181  ned_of_ecef_point_d(ned, def, &ecef);
182 }
183 
184 
185 /* geocentric latitude of geodetic latitude */
186 double gc_of_gd_lat_d(double gd_lat, double hmsl)
187 {
188  const double a = 6378137.0; /* earth semimajor axis in meters */
189  const double f = 1. / 298.257223563; /* reciprocal flattening */
190  const double c2 = (1. - f) * (1. - f);
191  /* geocentric latitude at the planet surface */
192  double ls = atan(c2 * tan(gd_lat));
193  return atan2(hmsl * sin(gd_lat) + a * sin(ls), hmsl * cos(gd_lat) + a * cos(ls));
194 }
195 
196 
197 #include "math/pprz_geodetic_utm.h"
198 
199 static inline double UNUSED isometric_latitude_d(double phi, double e)
200 {
201  return log(tan(M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
202 }
203 
204 static inline double isometric_latitude_fast_d(double phi)
205 {
206  return log(tan(M_PI_4 + phi / 2.0));
207 }
208 
209 static inline double inverse_isometric_latitude_d(double lat, double e, double epsilon)
210 {
211  double exp_l = exp(lat);
212  double phi0 = 2 * atan(exp_l) - M_PI_2;
213  double phi_;
214  uint8_t max_iter = 3; /* To be sure to return */
215 
216  do {
217  phi_ = phi0;
218  double sin_phi = e * sin(phi_);
219  phi0 = 2 * atan(pow((1 + sin_phi) / (1. - sin_phi), e / 2.) * exp_l) - M_PI_2;
220  max_iter--;
221  } while (max_iter && fabs(phi_ - phi0) > epsilon);
222 
223  return phi0;
224 }
225 
226 #define CI(v) { \
227  double tmp = v.x; \
228  v.x = -v.y; \
229  v.y = tmp; \
230  }
231 
232 #define CExp(v) { \
233  double e = exp(v.x); \
234  v.x = e*cosf(v.y); \
235  v.y = e*sinf(v.y); \
236  }
237 
238 #define CSin(v) { \
239  CI(v); \
240  struct DoubleVect2 vstar = {-v.x, -v.y}; \
241  CExp(v); \
242  CExp(vstar); \
243  VECT2_SUB(v, vstar); \
244  VECT2_SMUL(v, v, -0.5); \
245  CI(v); \
246  }
247 
248 void lla_of_utm_d(struct LlaCoor_d *lla, struct UtmCoor_d *utm)
249 {
250 
251  struct DoubleVect2 v = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST};
252  double scale = 1 / N / serie_coeff_proj_mercator[0];
253  VECT2_SMUL(v, v, scale);
254 
255  // first order taylor serie of something ?
256  struct DoubleVect2 v1;
257  VECT2_SMUL(v1, v, 2.);
258  CSin(v1);
260  VECT2_SUB(v, v1);
261 
262  double lambda_c = LambdaOfUtmZone(utm->zone);
263  lla->lon = lambda_c + atan(sinh(v.y) / cos(v.x));
264  double phi = asin(sin(v.x) / cosh(v.y));
265  double il = isometric_latitude_fast_d(phi);
266  lla->lat = inverse_isometric_latitude_d(il, E, 1e-8);
267 
268  // copy alt above reference ellipsoid
269  lla->alt = utm->alt;
270 
271 }
#define Q
Definition: hf_float.c:74
double z
in meters
double m[3 *3]
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:146
vector in North East Down coordinates Units: meters
float epsilon
void lla_of_utm_d(struct LlaCoor_d *lla, struct UtmCoor_d *utm)
double north
in meters
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
position in UTM coordinates Units: meters
double gc_of_gd_lat_d(double gd_lat, double hmsl)
#define DELTA_EAST
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
struct DoubleRMat ltp_of_ecef
rotation from ECEF to local frame
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:181
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
static double inverse_isometric_latitude_d(double lat, double e, double epsilon)
struct LlaCoor_d lla
origin of local frame in LLA
double alt
in meters above WGS84 reference ellipsoid
vector in Latitude, Longitude and Altitude
void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:41
Paparazzi double-precision floating point math for geodetic calculations.
void enu_of_lla_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct LlaCoor_d *lla)
#define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:464
double east
in meters
vector in East North Up coordinates Units: meters
double y
in meters
double x
in meters
void enu_of_ecef_vect_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
static double isometric_latitude_fast_d(double phi)
#define DELTA_NORTH
definition of the local (flat earth) coordinate system
vector in EarthCenteredEarthFixed coordinates
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:97
Constants UTM (Mercator) projections.
void ned_of_lla_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct LlaCoor_d *lla)
uint8_t zone
UTM zone number.
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:451
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
#define CSin(v)
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
unsigned char uint8_t
Definition: types.h:14
struct EcefCoor_d ecef
origin of local frame in ECEF
#define LambdaOfUtmZone(utm_zone)
static double UNUSED isometric_latitude_d(double phi, double e)
double lon
in radians
static const float serie_coeff_proj_mercator[5]
static uint16_t c2
Definition: baro_MS5534A.c:203
void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
#define E
void enu_of_ecef_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
#define N
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
#define VECT2_SUB(_a, _b)
Definition: pprz_algebra.h:79