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_int.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
22 #include "pprz_geodetic_int.h"
23 #include "pprz_algebra_int.h"
24 
25 
26 void ltp_of_ecef_rmat_from_lla_i(struct Int32Mat33* ltp_of_ecef, struct LlaCoor_i* lla) {
27 
28 #if USE_DOUBLE_PRECISION_TRIG
29  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
30  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
31  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
32  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
33 #else
34  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
35  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
36  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
37  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
38 #endif
39 
40  ltp_of_ecef->m[0] = -sin_lon;
41  ltp_of_ecef->m[1] = cos_lon;
42  ltp_of_ecef->m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
43  ltp_of_ecef->m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
44  ltp_of_ecef->m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
45  ltp_of_ecef->m[5] = cos_lat;
46  ltp_of_ecef->m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
47  ltp_of_ecef->m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
48  ltp_of_ecef->m[8] = sin_lat;
49 }
50 
51 void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
52 
53  /* store the origin of the tangeant plane */
54  VECT3_COPY(def->ecef, *ecef);
55  /* compute the lla representation of the origin */
56  lla_of_ecef_i(&def->lla, &def->ecef);
57  /* store the rotation matrix */
59 
60 }
61 
62 void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla) {
63 
64  /* store the origin of the tangeant plane */
65  LLA_COPY(def->lla, *lla);
66  /* compute the ecef representation of the origin */
67  ecef_of_lla_i(&def->ecef, &def->lla);
68  /* store the rotation matrix */
70 
71 }
72 
73 
79 void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
80 
81  struct EcefCoor_i delta;
82  VECT3_DIFF(delta, *ecef, def->ecef);
83  const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*delta.x +
84  (int64_t)def->ltp_of_ecef.m[1]*delta.y +
85  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
86  enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
87  const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*delta.x +
88  (int64_t)def->ltp_of_ecef.m[4]*delta.y +
89  (int64_t)def->ltp_of_ecef.m[5]*delta.z;
90  enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
91  const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*delta.x +
92  (int64_t)def->ltp_of_ecef.m[7]*delta.y +
93  (int64_t)def->ltp_of_ecef.m[8]*delta.z;
94  enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
95 
96 }
97 
98 
104 void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
105  struct EnuCoor_i enu;
106  enu_of_ecef_point_i(&enu, def, ecef);
107  ENU_OF_TO_NED(*ned, enu);
108 }
109 
110 
116 void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
117  struct EnuCoor_i enu_cm;
118  enu_of_ecef_point_i(&enu_cm, def, ecef);
119 
120  /* enu = (enu_cm / 100) << INT32_POS_FRAC
121  * to loose less range:
122  * enu_cm = enu << (INT32_POS_FRAC-2) / 25
123  * which puts max enu output Q23.8 range to 8388km / 25 = 335km
124  */
125  INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC-2);
126  VECT3_SDIV(*enu, *enu, 25);
127 }
128 
129 
135 void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
136  struct EnuCoor_i enu;
137  enu_of_ecef_pos_i(&enu, def, ecef);
138  ENU_OF_TO_NED(*ned, enu);
139 }
140 
141 
147 void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
148 
149  const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*ecef->x +
150  (int64_t)def->ltp_of_ecef.m[1]*ecef->y +
151  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
152  enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
153  const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*ecef->x +
154  (int64_t)def->ltp_of_ecef.m[4]*ecef->y +
155  (int64_t)def->ltp_of_ecef.m[5]*ecef->z;
156  enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
157  const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*ecef->x +
158  (int64_t)def->ltp_of_ecef.m[7]*ecef->y +
159  (int64_t)def->ltp_of_ecef.m[8]*ecef->z;
160  enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
161 
162 }
163 
164 
170 void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
171  struct EnuCoor_i enu;
172  enu_of_ecef_vect_i(&enu, def, ecef);
173  ENU_OF_TO_NED(*ned, enu);
174 }
175 
176 
182 void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
183 
184  const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0] * enu->x +
185  (int64_t)def->ltp_of_ecef.m[3] * enu->y +
186  (int64_t)def->ltp_of_ecef.m[6] * enu->z;
187  ecef->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
188 
189  const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[1] * enu->x +
190  (int64_t)def->ltp_of_ecef.m[4] * enu->y +
191  (int64_t)def->ltp_of_ecef.m[7] * enu->z;
192  ecef->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
193 
194  /* first element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ENU_to_ECEF */
195  const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[5] * enu->y +
196  (int64_t)def->ltp_of_ecef.m[8] * enu->z;
197  ecef->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
198 
199 }
200 
201 
207 void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
208  struct EnuCoor_i enu;
209  ENU_OF_TO_NED(enu, *ned);
210  ecef_of_enu_vect_i(ecef, def, &enu);
211 }
212 
213 
219 void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
220  ecef_of_enu_vect_i(ecef, def, enu);
221  INT32_VECT3_ADD(*ecef, def->ecef);
222 }
223 
224 
230 void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
231  struct EnuCoor_i enu;
232  ENU_OF_TO_NED(enu, *ned);
233  ecef_of_enu_point_i(ecef, def, &enu);
234 }
235 
236 
242 void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
243  /* enu_cm = (enu * 100) >> INT32_POS_FRAC
244  * to loose less range:
245  * enu_cm = (enu * 25) >> (INT32_POS_FRAC-2)
246  * which puts max enu input Q23.8 range to 8388km / 25 = 335km
247  */
248  struct EnuCoor_i enu_cm;
249  VECT3_SMUL(enu_cm, *enu, 25);
250  INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC-2);
251  ecef_of_enu_vect_i(ecef, def, &enu_cm);
252  INT32_VECT3_ADD(*ecef, def->ecef);
253 }
254 
255 
261 void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
262  struct EnuCoor_i enu;
263  ENU_OF_TO_NED(enu, *ned);
264  ecef_of_enu_pos_i(ecef, def, &enu);
265 }
266 
267 
268 void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
269  struct EcefCoor_i ecef;
270  ecef_of_lla_i(&ecef,lla);
271  enu_of_ecef_point_i(enu,def,&ecef);
272 }
273 
274 void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
275  struct EcefCoor_i ecef;
276  ecef_of_lla_i(&ecef,lla);
277  ned_of_ecef_point_i(ned,def,&ecef);
278 }
279 
280 void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
281  struct EcefCoor_i ecef;
282  ecef_of_lla_i(&ecef,lla);
283  enu_of_ecef_vect_i(enu,def,&ecef);
284 }
285 
286 void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
287  struct EcefCoor_i ecef;
288  ecef_of_lla_i(&ecef,lla);
289  ned_of_ecef_vect_i(ned,def,&ecef);
290 }
291 
292 /*
293  For now we cheat and call the floating point version
294  Anyone up for writing it in fixed point ?
295 */
296 #include "pprz_geodetic_float.h"
297 #include "pprz_geodetic_double.h"
298 
299 void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) {
300 
301  /* convert our input to floating point */
302  struct EcefCoor_d in_d;
303  in_d.x = M_OF_CM((double)in->x);
304  in_d.y = M_OF_CM((double)in->y);
305  in_d.z = M_OF_CM((double)in->z);
306  /* calls the floating point transformation */
307  struct LlaCoor_d out_d;
308  lla_of_ecef_d(&out_d, &in_d);
309  /* convert the output to fixed point */
310  out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_d.lon));
311  out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_d.lat));
312  out->alt = (int32_t)MM_OF_M(out_d.alt);
313 
314 }
315 
316 void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) {
317 
318  /* convert our input to floating point */
319  struct LlaCoor_d in_d;
320  in_d.lon = RAD_OF_EM7DEG((double)in->lon);
321  in_d.lat = RAD_OF_EM7DEG((double)in->lat);
322  in_d.alt = M_OF_MM((double)in->alt);
323  /* calls the floating point transformation */
324  struct EcefCoor_d out_d;
325  ecef_of_lla_d(&out_d, &in_d);
326  /* convert the output to fixed point */
327  out->x = (int32_t)CM_OF_M(out_d.x);
328  out->y = (int32_t)CM_OF_M(out_d.y);
329  out->z = (int32_t)CM_OF_M(out_d.z);
330 
331 }
#define INT32_VECT3_ADD(_a, _b)
int32_t y
in centimeters
void enu_of_ecef_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a ECEF position to local ENU.
int32_t lat
in degrees*1e7
void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a point in local ENU to ECEF.
struct Int32Mat33 ltp_of_ecef
Rotation matrix.
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:5
double alt
in meters above WGS84 reference ellipsoid
vector in EarthCenteredEarthFixed coordinates
void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Rotate a vector from NED to ECEF.
#define INT32_POS_FRAC
double x
in meters
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:178
void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Rotate a vector from ENU to ECEF.
#define M_OF_MM(_mm)
void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a local ENU position to ECEF.
struct LlaCoor_i lla
Reference point in lla.
void enu_of_ecef_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to ENU.
vector in Latitude, Longitude and Altitude
void ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a ECEF position to local NED.
signed long long int64_t
Definition: types.h:21
#define CM_OF_M(_m)
#define INT32_VECT3_LSHIFT(_o, _i, _l)
int32_t z
in centimeters
void ecef_of_ned_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Convert a local NED position to ECEF.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
int32_t z
Up.
Paparazzi double-precision floating point math for geodetic calculations.
vector in North East Down coordinates
Paparazzi floating point math for geodetic calculations.
void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
definition of the local (flat earth) coordinate system
vector in EarthCenteredEarthFixed coordinates
#define MM_OF_M(_m)
#define INT32_VECT3_RSHIFT(_o, _i, _r)
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
#define BFP_OF_REAL(_vr, _frac)
int32_t alt
in millimeters above WGS84 reference ellipsoid
double lon
in radians
double y
in meters
void ltp_of_ecef_rmat_from_lla_i(struct Int32Mat33 *ltp_of_ecef, struct LlaCoor_i *lla)
void enu_of_ecef_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local ENU.
Paparazzi fixed point math for geodetic calculations.
int32_t x
in centimeters
signed long int32_t
Definition: types.h:19
void ecef_of_ned_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Convert a point in local NED to ECEF.
#define HIGH_RES_TRIG_FRAC
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
double lat
in radians
int32_t lon
in degrees*1e7
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:171
double z
in meters
struct EcefCoor_i ecef
Reference point in ecef.
#define RAD_OF_EM7DEG(_r)
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
#define M_OF_CM(_cm)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:129
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
int32_t m[3 *3]
int32_t y
North.
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:185
vector in Latitude, Longitude and Altitude
vector in East North Up coordinates
#define EM7DEG_OF_RAD(_r)
int32_t x
East.
Paparazzi fixed point algebra.
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:17