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