Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 = sqrtf(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 /* geocentric latitude of geodetic latitude */
131 double gc_of_gd_lat_d(double gd_lat, double hmsl) {
132  const double a = 6378137.0; /* earth semimajor axis in meters */
133  const double f = 1./298.257223563; /* reciprocal flattening */
134  const double c2 = (1.-f)*(1.-f);
135  /* geocentric latitude at the planet surface */
136  double ls = atan(c2*tan(gd_lat));
137  return atan2(hmsl*sin(gd_lat) + a*sin(ls), hmsl*cos(gd_lat) + a*cos(ls));
138 }
139 
140 
141 #include "math/pprz_geodetic_utm.h"
142 
143 static inline double isometric_latitude_d(double phi, double e) {
144  return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
145 }
146 
147 static inline double isometric_latitude_fast_d(double phi) {
148  return log (tan (M_PI_4 + phi / 2.0));
149 }
150 
151 static inline double inverse_isometric_latitude_d(double lat, double e, double epsilon) {
152  double exp_l = exp(lat);
153  double phi0 = 2 * atan(exp_l) - M_PI_2;
154  double phi_;
155  uint8_t max_iter = 3; /* To be sure to return */
156 
157  do {
158  phi_ = phi0;
159  double sin_phi = e * sin(phi_);
160  phi0 = 2 * atan (pow((1 + sin_phi) / (1. - sin_phi), e/2.) * exp_l) - M_PI_2;
161  max_iter--;
162  }
163  while (max_iter && fabs(phi_ - phi0) > epsilon);
164 
165  return phi0;
166 }
167 
168 #define CI(v) { \
169  double tmp = v.x; \
170  v.x = -v.y; \
171  v.y = tmp; \
172  }
173 
174 #define CExp(v) { \
175  double e = exp(v.x); \
176  v.x = e*cosf(v.y); \
177  v.y = e*sinf(v.y); \
178  }
179 
180 #define CSin(v) { \
181  CI(v); \
182  struct DoubleVect2 vstar = {-v.x, -v.y}; \
183  CExp(v); \
184  CExp(vstar); \
185  VECT2_SUB(v, vstar); \
186  VECT2_SMUL(v, v, -0.5); \
187  CI(v); \
188  }
189 
190 void lla_of_utm_d(struct LlaCoor_d* lla, struct UtmCoor_d* utm) {
191 
192  struct DoubleVect2 v = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST};
193  double scale = 1 / N / serie_coeff_proj_mercator[0];
194  VECT2_SMUL(v, v, scale);
195 
196  // first order taylor serie of something ?
197  struct DoubleVect2 v1;
198  VECT2_SMUL(v1, v, 2.);
199  CSin(v1);
201  VECT2_SUB(v, v1);
202 
203  double lambda_c = LambdaOfUtmZone(utm->zone);
204  lla->lon = lambda_c + atan(sinh(v.y) / cos(v.x));
205  double phi = asin (sin(v.x) / cosh(v.y));
206  double il = isometric_latitude_fast_d(phi);
207  lla->lat = inverse_isometric_latitude_d(il, E, 1e-8);
208 
209  // copy alt above reference ellipsoid
210  lla->alt = utm->alt;
211 
212 }
#define Q
Definition: hf_float.c:52
#define N
vector in North East Down coordinates Units: meters
struct LlaCoor_d lla
origin of local frame in 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:401
#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)
static double isometric_latitude_d(double phi, double e)
#define VECT2_SUB(_a, _b)
Definition: pprz_algebra.h:57
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
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:120
double north
in meters
#define CSin(v)
unsigned char uint8_t
Definition: types.h:14
double lat
in radians
#define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:414
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:155
static uint16_t c2
Definition: baro_MS5534A.c:197
double z
in meters
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:75
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:113
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
static struct point c
Definition: discsurvey.c:13
uint8_t zone
UTM zone number.
vector in Latitude, Longitude and Altitude
position in UTM coordinates Units: meters