Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_geodetic_float.c
Go to the documentation of this file.
1 #include "pprz_geodetic_float.h"
2 
3 #include "pprz_algebra_float.h"
4 #include <math.h>
5 
6 /* for ecef_of_XX functions the double versions are needed */
7 #include "pprz_geodetic_double.h"
8 
9 void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) {
10 
11  /* store the origin of the tangeant plane */
12  VECT3_COPY(def->ecef, *ecef);
13  /* compute the lla representation of the origin */
14  lla_of_ecef_f(&def->lla, &def->ecef);
15  /* store the rotation matrix */
16  const float sin_lat = sinf(def->lla.lat);
17  const float cos_lat = cosf(def->lla.lat);
18  const float sin_lon = sinf(def->lla.lon);
19  const float cos_lon = cosf(def->lla.lon);
20  def->ltp_of_ecef.m[0] = -sin_lon;
21  def->ltp_of_ecef.m[1] = cos_lon;
22  def->ltp_of_ecef.m[2] = 0.; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
23  def->ltp_of_ecef.m[3] = -sin_lat*cos_lon;
24  def->ltp_of_ecef.m[4] = -sin_lat*sin_lon;
25  def->ltp_of_ecef.m[5] = cos_lat;
26  def->ltp_of_ecef.m[6] = cos_lat*cos_lon;
27  def->ltp_of_ecef.m[7] = cos_lat*sin_lon;
28  def->ltp_of_ecef.m[8] = sin_lat;
29 
30 }
31 
32 void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla) {
33  /* store the origin of the tangeant plane */
34  LLA_COPY(def->lla, *lla);
35  /* compute the ecef representation of the origin */
36  ecef_of_lla_f(&def->ecef, &def->lla);
37 
38  /* store the rotation matrix */
39  const float sin_lat = sinf(def->lla.lat);
40  const float cos_lat = cosf(def->lla.lat);
41  const float sin_lon = sinf(def->lla.lon);
42  const float cos_lon = cosf(def->lla.lon);
43 
44  def->ltp_of_ecef.m[0] = -sin_lon;
45  def->ltp_of_ecef.m[1] = cos_lon;
46  def->ltp_of_ecef.m[2] = 0.; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
47  def->ltp_of_ecef.m[3] = -sin_lat*cos_lon;
48  def->ltp_of_ecef.m[4] = -sin_lat*sin_lon;
49  def->ltp_of_ecef.m[5] = cos_lat;
50  def->ltp_of_ecef.m[6] = cos_lat*cos_lon;
51  def->ltp_of_ecef.m[7] = cos_lat*sin_lon;
52  def->ltp_of_ecef.m[8] = sin_lat;
53 }
54 
55 void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) {
56  struct EcefCoor_f delta;
57  VECT3_DIFF(delta, *ecef, def->ecef);
58  MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta);
59 }
60 
61 void ned_of_ecef_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef) {
62  struct EnuCoor_f enu;
63  enu_of_ecef_point_f(&enu, def, ecef);
64  ENU_OF_TO_NED(*ned, enu);
65 }
66 
67 
68 void enu_of_ecef_vect_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) {
69  MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, *ecef);
70 }
71 
72 void ned_of_ecef_vect_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef) {
73  struct EnuCoor_f enu;
74  enu_of_ecef_vect_f(&enu, def, ecef);
75  ENU_OF_TO_NED(*ned, enu);
76 }
77 
78 void enu_of_lla_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct LlaCoor_f* lla) {
79  struct EcefCoor_f ecef;
80  ecef_of_lla_f(&ecef,lla);
81  enu_of_ecef_point_f(enu,def,&ecef);
82 }
83 
84 void ned_of_lla_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct LlaCoor_f* lla) {
85  struct EcefCoor_f ecef;
86  ecef_of_lla_f(&ecef,lla);
87  ned_of_ecef_point_f(ned,def,&ecef);
88 }
89 
90 /*
91  * not enought precision with float - use double
92  */
93 void ecef_of_enu_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) {
94  /* convert used floats to double */
95  struct DoubleMat33 ltp_of_ecef_d;
96  ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0];
97  ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1];
98  ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2];
99  ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3];
100  ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4];
101  ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5];
102  ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6];
103  ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7];
104  ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8];
105  struct EnuCoor_f enu_d;
106  enu_d.x = (double) enu->x;
107  enu_d.y = (double) enu->y;
108  enu_d.z = (double) enu->z;
109 
110  /* compute in double */
111  struct EcefCoor_d ecef_d;
112  MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d);
113 
114  /* convert result back to float and add it*/
115  ecef->x = (float) ecef_d.x + def->ecef.x;
116  ecef->y = (float) ecef_d.y + def->ecef.y;
117  ecef->z = (float) ecef_d.z + def->ecef.z;
118 }
119 
120 void ecef_of_ned_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned) {
121  struct EnuCoor_f enu;
122  ENU_OF_TO_NED(enu, *ned);
123  ecef_of_enu_point_f(ecef, def, &enu);
124 }
125 
126 void ecef_of_enu_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) {
127  /* convert used floats to double */
128  struct DoubleMat33 ltp_of_ecef_d;
129  ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0];
130  ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1];
131  ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2];
132  ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3];
133  ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4];
134  ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5];
135  ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6];
136  ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7];
137  ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8];
138  struct EnuCoor_f enu_d;
139  enu_d.x = (double) enu->x;
140  enu_d.y = (double) enu->y;
141  enu_d.z = (double) enu->z;
142 
143  /* compute in double */
144  struct EcefCoor_d ecef_d;
145  MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d);
146 
147  /* convert result back to float*/
148  ecef->x = (float) ecef_d.x;
149  ecef->y = (float) ecef_d.y;
150  ecef->z = (float) ecef_d.z;
151 }
152 
153 void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned) {
154  struct EnuCoor_f enu;
155  ENU_OF_TO_NED(enu, *ned);
156  ecef_of_enu_vect_f(ecef, def, &enu);
157 }
158 /* end use double versions */
159 
160 
161 
162 
163 /* http://en.wikipedia.org/wiki/Geodetic_system */
164 void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in) {
165 
166  // FIXME : make an ellipsoid struct
167  static const float a = 6378137.0; /* earth semimajor axis in meters */
168  static const float f = 1./298.257223563; /* reciprocal flattening */
169  const float b = a*(1.-f); /* semi-minor axis */
170  const float b2 = b*b;
171 
172  const float e2 = 2.*f-(f*f); /* first eccentricity squared */
173  const float ep2 = f*(2.-f)/((1.-f)*(1.-f)); /* second eccentricity squared */
174  const float E2 = a*a - b2;
175 
176 
177  const float z2 = in->z*in->z;
178  const float r2 = in->x*in->x+in->y*in->y;
179  const float r = sqrtf(r2);
180  const float F = 54.*b2*z2;
181  const float G = r2 + (1-e2)*z2 - e2*E2;
182  const float c = (e2*e2*F*r2)/(G*G*G);
183  const float s = powf( (1 + c + sqrtf(c*c + 2*c)), 1./3.);
184  const float s1 = 1+s+1/s;
185  const float P = F/(3*s1*s1*G*G);
186  const float Q = sqrtf(1+2*e2*e2*P);
187  const float ro = -(e2*P*r)/(1+Q) + sqrtf((a*a/2)*(1+1/Q) - ((1-e2)*P*z2)/(Q*(1+Q)) - P*r2/2);
188  const float tmp = (r - e2*ro)*(r - e2*ro);
189  const float U = sqrtf( tmp + z2 );
190  const float V = sqrtf( tmp + (1-e2)*z2 );
191  const float zo = (b2*in->z)/(a*V);
192 
193  out->alt = U*(1 - b2/(a*V));
194  out->lat = atanf((in->z + ep2*zo)/r);
195  out->lon = atan2f(in->y,in->x);
196 
197 }
198 
199 void ecef_of_lla_f(struct EcefCoor_f* out, struct LlaCoor_f* in) {
200 
201  // FIXME : make an ellipsoid struct
202  static const float a = 6378137.0; /* earth semimajor axis in meters */
203  static const float f = 1./298.257223563; /* reciprocal flattening */
204  const float e2 = 2.*f-(f*f); /* first eccentricity squared */
205 
206  const float sin_lat = sinf(in->lat);
207  const float cos_lat = cosf(in->lat);
208  const float sin_lon = sinf(in->lon);
209  const float cos_lon = cosf(in->lon);
210  const float chi = sqrtf(1. - e2*sin_lat*sin_lat);
211  const float a_chi = a / chi;
212 
213  out->x = (a_chi + in->alt) * cos_lat * cos_lon;
214  out->y = (a_chi + in->alt) * cos_lat * sin_lon;
215  out->z = (a_chi*(1. - e2) + in->alt) * sin_lat;
216 }
217 
218 
219 
220 
221 #include "math/pprz_geodetic_utm.h"
222 
223 struct complex { float re; float im; };
224 #define CScal(k, z) { z.re *= k; z.im *= k; }
225 #define CAdd(z1, z2) { z2.re += z1.re; z2.im += z1.im; }
226 #define CSub(z1, z2) { z2.re -= z1.re; z2.im -= z1.im; }
227 #define CI(z) { float tmp = z.re; z.re = - z.im; z.im = tmp; }
228 #define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); }
229 /* Expanded #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; CExp(z); CExp(_z); CSub(_z, z); CScal(-0.5, z); CI(z); } */
230 
231 #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cos(z.im); z.re = e*cos_z_im; float sin_z_im = sin(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); }
232 
233 
234 static inline float isometric_latitude_f(float phi, float e) {
235  return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
236 }
237 
238 static inline float isometric_latitude_fast_f(float phi) {
239  return log (tan (M_PI_4 + phi / 2.0));
240 }
241 
242 static inline float inverse_isometric_latitude_f(float lat, float e, float epsilon) {
243  float exp_l = exp(lat);
244  float phi0 = 2 * atan(exp_l) - M_PI_2;
245  float phi_;
246  uint8_t max_iter = 3; /* To be sure to return */
247 
248  do {
249  phi_ = phi0;
250  float sin_phi = e * sin(phi_);
251  phi0 = 2 * atan (pow((1 + sin_phi) / (1. - sin_phi), e/2.) * exp_l) - M_PI_2;
252  max_iter--;
253  } while (max_iter && fabs(phi_ - phi0) > epsilon);
254  return phi0;
255 }
256 
257 void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla) {
258  float lambda_c = LambdaOfUtmZone(utm->zone);
259  float ll = isometric_latitude_f(lla->lat , E);
260  float dl = lla->lon - lambda_c;
261  float phi_ = asin(sin(dl) / cosh(ll));
262  float ll_ = isometric_latitude_fast_f(phi_);
263  float lambda_ = atan(sinh(ll) / cos(dl));
264  struct complex z_ = { lambda_, ll_ };
266  uint8_t k;
267  for(k = 1; k < 3; k++) {
268  struct complex z = { lambda_, ll_ };
269  CScal(2*k, z);
270  CSin(z);
272  CAdd(z, z_);
273  }
274  CScal(N, z_);
275  utm->east = DELTA_EAST + z_.im;
276  utm->north = DELTA_NORTH + z_.re;
277 
278  // copy alt above reference ellipsoid
279  utm->alt = lla->alt;
280 }
281 
282 void lla_of_utm_f(struct LlaCoor_f* lla, struct UtmCoor_f* utm) {
283  float scale = 1 / N / serie_coeff_proj_mercator[0];
284  float real = (utm->north - DELTA_NORTH) * scale;
285  float img = (utm->east - DELTA_EAST) * scale;
286  struct complex z = { real, img };
287 
288  uint8_t k;
289  for(k = 1; k < 2; k++) {
290  struct complex z_ = { real, img };
291  CScal(2*k, z_);
292  CSin(z_);
294  CSub(z_, z);
295  }
296 
297  float lambda_c = LambdaOfUtmZone(utm->zone);
298  lla->lon = lambda_c + atan (sinh(z.im) / cos(z.re));
299  float phi_ = asin (sin(z.re) / cosh(z.im));
300  float il = isometric_latitude_fast_f(phi_);
301  lla->lat = inverse_isometric_latitude_f(il, E, 1e-8);
302 
303  // copy alt above reference ellipsoid
304  lla->alt = utm->alt;
305 }
#define Q
Definition: hf_float.c:57
#define N
vector in EarthCenteredEarthFixed coordinates
void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:5
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
#define CScal(k, z)
#define DELTA_NORTH
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
float y
in meters
void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
double x
in meters
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
float z
in meters
struct FloatMat33 ltp_of_ecef
rotation from ECEF to local frame
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
#define DELTA_EAST
float m[3 *3]
#define LambdaOfUtmZone(utm_zone)
float lat
in radians
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
static const float serie_coeff_proj_mercator[5]
void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
#define CSin(z)
void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef)
float y
in meters
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:399
#define E
struct EcefCoor_f ecef
origin of local frame in ECEF
static float inverse_isometric_latitude_f(float lat, float e, float epsilon)
vector in Latitude, Longitude and Altitude
Paparazzi double-precision floating point math for geodetic calculations.
double m[3 *3]
Paparazzi floating point math for geodetic calculations.
#define CSub(z1, z2)
uint8_t zone
UTM zone number.
float x
in meters
static float isometric_latitude_fast_f(float phi)
Paparazzi floating point algebra.
vector in North East Down coordinates Units: meters
vector in EarthCenteredEarthFixed coordinates
float north
in meters
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
position in UTM coordinates Units: meters
float x
in meters
double y
in meters
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
float alt
in meters above WGS84 reference ellipsoid
void ltp_def_from_lla_f(struct LtpDef_f *def, struct LlaCoor_f *lla)
unsigned char uint8_t
Definition: types.h:14
static float isometric_latitude_f(float phi, float e)
#define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:412
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:153
double z
in meters
definition of the local (flat earth) coordinate system
struct LlaCoor_f lla
origin of local frame in LLA
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:111
vector in East North Up coordinates Units: meters
float lon
in radians
static struct point c
Definition: discsurvey.c:39
static const float serie_coeff_proj_mercator_inverse[5]
float east
in meters
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
float alt
in meters above WGS84 reference ellipsoid
float z
in meters
#define CAdd(z1, z2)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
void ecef_of_enu_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:17