Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
pprz_geodetic_float.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_float.h"
29 
30 #include "pprz_algebra_float.h"
31 #include <math.h>
32 
33 /* for ecef_of_XX functions the double versions are needed */
34 #include "pprz_geodetic_double.h"
35 
36 void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef)
37 {
38  /* store the origin of the tangent plane */
39  VECT3_COPY(def->ecef, *ecef);
40  /* compute the lla representation of the origin */
41  lla_of_ecef_f(&def->lla, &def->ecef);
42  /* store the rotation matrix */
43  const float sin_lat = sinf(def->lla.lat);
44  const float cos_lat = cosf(def->lla.lat);
45  const float sin_lon = sinf(def->lla.lon);
46  const float cos_lon = cosf(def->lla.lon);
47  def->ltp_of_ecef.m[0] = -sin_lon;
48  def->ltp_of_ecef.m[1] = cos_lon;
49  /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
50  def->ltp_of_ecef.m[2] = 0.;
51  def->ltp_of_ecef.m[3] = -sin_lat * cos_lon;
52  def->ltp_of_ecef.m[4] = -sin_lat * sin_lon;
53  def->ltp_of_ecef.m[5] = cos_lat;
54  def->ltp_of_ecef.m[6] = cos_lat * cos_lon;
55  def->ltp_of_ecef.m[7] = cos_lat * sin_lon;
56  def->ltp_of_ecef.m[8] = sin_lat;
57 
58 }
59 
60 void ltp_def_from_lla_f(struct LtpDef_f *def, struct LlaCoor_f *lla)
61 {
62  /* store the origin of the tangent plane */
63  LLA_COPY(def->lla, *lla);
64  /* compute the ecef representation of the origin */
65  ecef_of_lla_f(&def->ecef, &def->lla);
66 
67  /* store the rotation matrix */
68  const float sin_lat = sinf(def->lla.lat);
69  const float cos_lat = cosf(def->lla.lat);
70  const float sin_lon = sinf(def->lla.lon);
71  const float cos_lon = cosf(def->lla.lon);
72 
73  def->ltp_of_ecef.m[0] = -sin_lon;
74  def->ltp_of_ecef.m[1] = cos_lon;
75  /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
76  def->ltp_of_ecef.m[2] = 0.;
77  def->ltp_of_ecef.m[3] = -sin_lat * cos_lon;
78  def->ltp_of_ecef.m[4] = -sin_lat * sin_lon;
79  def->ltp_of_ecef.m[5] = cos_lat;
80  def->ltp_of_ecef.m[6] = cos_lat * cos_lon;
81  def->ltp_of_ecef.m[7] = cos_lat * sin_lon;
82  def->ltp_of_ecef.m[8] = sin_lat;
83 }
84 
85 void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
86 {
87  struct EcefCoor_f delta;
88  VECT3_DIFF(delta, *ecef, def->ecef);
89  MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta);
90 }
91 
92 void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
93 {
94  struct EnuCoor_f enu;
95  enu_of_ecef_point_f(&enu, def, ecef);
96  ENU_OF_TO_NED(*ned, enu);
97 }
98 
99 
100 void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
101 {
102  MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, *ecef);
103 }
104 
105 void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
106 {
107  struct EnuCoor_f enu;
108  enu_of_ecef_vect_f(&enu, def, ecef);
109  ENU_OF_TO_NED(*ned, enu);
110 }
111 
112 void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
113 {
114  struct EcefCoor_f ecef;
115  ecef_of_lla_f(&ecef, lla);
116  enu_of_ecef_point_f(enu, def, &ecef);
117 }
118 
119 void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
120 {
121  struct EcefCoor_f ecef;
122  ecef_of_lla_f(&ecef, lla);
123  ned_of_ecef_point_f(ned, def, &ecef);
124 }
125 
126 /*
127  * not enough precision with float - use double
128  */
129 void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
130 {
131  /* convert used floats to double */
132  struct DoubleRMat ltp_of_ecef_d;
133  ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0];
134  ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1];
135  ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2];
136  ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3];
137  ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4];
138  ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5];
139  ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6];
140  ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7];
141  ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8];
142  struct EnuCoor_f enu_d;
143  enu_d.x = (double) enu->x;
144  enu_d.y = (double) enu->y;
145  enu_d.z = (double) enu->z;
146 
147  /* compute in double */
148  struct EcefCoor_d ecef_d;
149  MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d);
150 
151  /* convert result back to float and add it*/
152  ecef->x = (float) ecef_d.x + def->ecef.x;
153  ecef->y = (float) ecef_d.y + def->ecef.y;
154  ecef->z = (float) ecef_d.z + def->ecef.z;
155 }
156 
157 void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
158 {
159  struct EnuCoor_f enu;
160  ENU_OF_TO_NED(enu, *ned);
161  ecef_of_enu_point_f(ecef, def, &enu);
162 }
163 
164 void ecef_of_enu_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
165 {
166  /* convert used floats to double */
167  struct DoubleRMat ltp_of_ecef_d;
168  ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0];
169  ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1];
170  ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2];
171  ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3];
172  ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4];
173  ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5];
174  ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6];
175  ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7];
176  ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8];
177  struct EnuCoor_f enu_d;
178  enu_d.x = (double) enu->x;
179  enu_d.y = (double) enu->y;
180  enu_d.z = (double) enu->z;
181 
182  /* compute in double */
183  struct EcefCoor_d ecef_d;
184  MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d);
185 
186  /* convert result back to float*/
187  ecef->x = (float) ecef_d.x;
188  ecef->y = (float) ecef_d.y;
189  ecef->z = (float) ecef_d.z;
190 }
191 
192 void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
193 {
194  struct EnuCoor_f enu;
195  ENU_OF_TO_NED(enu, *ned);
196  ecef_of_enu_vect_f(ecef, def, &enu);
197 }
198 /* end use double versions */
199 
200 
201 
202 
203 /* http://en.wikipedia.org/wiki/Geodetic_system */
204 void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
205 {
206 
207  // FIXME : make an ellipsoid struct
208  static const float a = 6378137.0; /* earth semimajor axis in meters */
209  static const float f = 1. / 298.257223563; /* reciprocal flattening */
210  const float b = a * (1. - f); /* semi-minor axis */
211  const float b2 = b * b;
212 
213  const float e2 = 2.*f - (f * f); /* first eccentricity squared */
214  const float ep2 = f * (2. - f) / ((1. - f) * (1. - f)); /* second eccentricity squared */
215  const float E2 = a * a - b2;
216 
217 
218  const float z2 = in->z * in->z;
219  const float r2 = in->x * in->x + in->y * in->y;
220  const float r = sqrtf(r2);
221  const float F = 54.*b2 * z2;
222  const float G = r2 + (1 - e2) * z2 - e2 * E2;
223  const float c = (e2 * e2 * F * r2) / (G * G * G);
224  const float s = powf((1 + c + sqrtf(c * c + 2 * c)), 1. / 3.);
225  const float s1 = 1 + s + 1 / s;
226  const float P = F / (3 * s1 * s1 * G * G);
227  const float Q = sqrtf(1 + 2 * e2 * e2 * P);
228  const float ro = -(e2 * P * r) / (1 + Q) + sqrtf((a * a / 2) * (1 + 1 / Q) - ((1 - e2) * P * z2) / (Q *
229  (1 + Q)) - P * r2 / 2);
230  const float tmp = (r - e2 * ro) * (r - e2 * ro);
231  const float U = sqrtf(tmp + z2);
232  const float V = sqrtf(tmp + (1 - e2) * z2);
233  const float zo = (b2 * in->z) / (a * V);
234 
235  out->alt = U * (1 - b2 / (a * V));
236  out->lat = atanf((in->z + ep2 * zo) / r);
237  out->lon = atan2f(in->y, in->x);
238 
239 }
240 
241 void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
242 {
243 
244  // FIXME : make an ellipsoid struct
245  static const float a = 6378137.0; /* earth semimajor axis in meters */
246  static const float f = 1. / 298.257223563; /* reciprocal flattening */
247  const float e2 = 2.*f - (f * f); /* first eccentricity squared */
248 
249  const float sin_lat = sinf(in->lat);
250  const float cos_lat = cosf(in->lat);
251  const float sin_lon = sinf(in->lon);
252  const float cos_lon = cosf(in->lon);
253  const float chi = sqrtf(1. - e2 * sin_lat * sin_lat);
254  const float a_chi = a / chi;
255 
256  out->x = (a_chi + in->alt) * cos_lat * cos_lon;
257  out->y = (a_chi + in->alt) * cos_lat * sin_lon;
258  out->z = (a_chi * (1. - e2) + in->alt) * sin_lat;
259 }
260 
261 
262 
263 
264 #include "math/pprz_geodetic_utm.h"
265 
266 struct complex { float re; float im; };
267 #define CScal(k, z) { z.re *= k; z.im *= k; }
268 #define CAdd(z1, z2) { z2.re += z1.re; z2.im += z1.im; }
269 #define CSub(z1, z2) { z2.re -= z1.re; z2.im -= z1.im; }
270 #define CI(z) { float tmp = z.re; z.re = - z.im; z.im = tmp; }
271 #define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); }
272 /* 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); } */
273 
274 #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cosf(z.im); z.re = e*cos_z_im; float sin_z_im = sinf(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); }
275 
276 
277 static inline float isometric_latitude_f(float phi, float e)
278 {
279  return logf(tanf(M_PI_4 + phi / 2.0)) - e / 2.0 * logf((1.0 + e * sinf(phi)) / (1.0 - e * sinf(phi)));
280 }
281 
282 static inline float isometric_latitude_fast_f(float phi)
283 {
284  return logf(tanf(M_PI_4 + phi / 2.0));
285 }
286 
287 static inline float inverse_isometric_latitude_f(float lat, float e, float epsilon)
288 {
289  float exp_l = expf(lat);
290  float phi0 = 2 * atanf(exp_l) - M_PI_2;
291  float phi_;
292  uint8_t max_iter = 3; /* To be sure to return */
293 
294  do {
295  phi_ = phi0;
296  float sin_phi = e * sinf(phi_);
297  phi0 = 2 * atanf(powf((1 + sin_phi) / (1. - sin_phi), e / 2.) * exp_l) - M_PI_2;
298  max_iter--;
299  } while (max_iter && fabsf(phi_ - phi0) > epsilon);
300  return phi0;
301 }
302 
303 /* Convert lla to utm (float).
304  * Note this conversion is not very accurate. If high accuracy needed use lla_of_utm_d.
305  * @param[out] utm position in m, alt is copied directly from lla
306  * @param[in] lla position in rad, alt in m
307  */
308 void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
309 {
310  // compute zone if not initialised
311  if (utm->zone == 0) {
312  utm->zone = UtmZoneOfLlaLonRad(lla->lon);
313  }
314 
315  float lambda_c = LambdaOfUtmZone(utm->zone);
316  float ll = isometric_latitude_f(lla->lat , E);
317  float dl = lla->lon - lambda_c;
318  float phi_ = asinf(sinf(dl) / coshf(ll));
319  float ll_ = isometric_latitude_fast_f(phi_);
320  float lambda_ = atanf(sinhf(ll) / cosf(dl));
321  struct complex z_ = { lambda_, ll_ };
323  int8_t k;
324  for (k = 1; k < 3; k++) {
325  struct complex z = { lambda_, ll_ };
326  CScal(2.*k, z);
327  CSin(z);
329  CAdd(z, z_);
330  }
331  CScal(N, z_);
332  utm->east = DELTA_EAST + z_.im;
333  utm->north = DELTA_NORTH + z_.re;
334 
335  // copy alt above reference ellipsoid
336  utm->alt = lla->alt;
337 }
338 
339 /* Convert utm to lla (float).
340  * Note this conversion is not very accurate. If high accuracy needed use lla_of_utm_d.
341  * @param[out] lla position in rad, alt is copied directly from utm
342  * @param[in] utm position in m, alt in m
343  */
344 void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
345 {
346  float scale = 1 / N / serie_coeff_proj_mercator[0];
347  float real = (utm->north - DELTA_NORTH) * scale;
348  float img = (utm->east - DELTA_EAST) * scale;
349  struct complex z = { real, img };
350 
351  int8_t k;
352  for (k = 1; k < 2; k++) {
353  struct complex z_ = { real, img };
354  CScal(2.*k, z_);
355  CSin(z_);
357  CSub(z_, z);
358  }
359 
360  float lambda_c = LambdaOfUtmZone(utm->zone);
361  lla->lon = lambda_c + atanf(sinhf(z.im) / cosf(z.re));
362  float phi_ = asinf(sinf(z.re) / coshf(z.im));
363  float il = isometric_latitude_fast_f(phi_);
364  lla->lat = inverse_isometric_latitude_f(il, E, 1e-8);
365 
366  // copy alt above reference ellipsoid
367  lla->alt = utm->alt;
368 }
static const float scale[]
double m[3 *3]
rotation matrix
float m[3 *3]
#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 VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
double x
in meters
double y
in meters
double z
in meters
vector in EarthCenteredEarthFixed coordinates
#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
Paparazzi floating point algebra.
Paparazzi double-precision floating point math for geodetic calculations.
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
#define CAdd(z1, z2)
static float isometric_latitude_fast_f(float phi)
static float isometric_latitude_f(float phi, float e)
static float inverse_isometric_latitude_f(float lat, float e, float epsilon)
void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
#define CSin(z)
void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void ecef_of_enu_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
void ltp_def_from_lla_f(struct LtpDef_f *def, struct LlaCoor_f *lla)
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
#define CScal(k, z)
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
#define CSub(z1, z2)
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Paparazzi floating point math for geodetic calculations.
float alt
in meters (normally above WGS84 reference ellipsoid)
float y
in meters
float x
in meters
float z
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
float lon
in radians
float x
in meters
float east
in meters
float lat
in radians
float north
in meters
float y
in meters
struct FloatRMat ltp_of_ecef
rotation from ECEF to local frame
struct EcefCoor_f ecef
origin of local frame in ECEF
float z
in meters
struct LlaCoor_f lla
origin of local frame in LLA
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
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