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_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 #define HIGH_RES_TRIG_FRAC 20
26 
27 void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
28 
29  /* store the origin of the tangeant plane */
30  VECT3_COPY(def->ecef, *ecef);
31  /* compute the lla representation of the origin */
32  lla_of_ecef_i(&def->lla, &def->ecef);
33  /* store the rotation matrix */
34 
35 #if 1
36  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
37  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
38  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
39  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
40 #else
41  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
42  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
43  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
44  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
45 #endif
46 
47 
48  def->ltp_of_ecef.m[0] = -sin_lon;
49  def->ltp_of_ecef.m[1] = cos_lon;
50  def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
51  def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
52  def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
53  def->ltp_of_ecef.m[5] = cos_lat;
54  def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
55  def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
56  def->ltp_of_ecef.m[8] = sin_lat;
57 
58 }
59 
60 void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla) {
61 
62  /* store the origin of the tangeant plane */
63  LLA_COPY(def->lla, *lla);
64  /* compute the ecef representation of the origin */
65  ecef_of_lla_i(&def->ecef, &def->lla);
66  /* store the rotation matrix */
67 
68 #if 1
69  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
70  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
71  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
72  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
73 #else
74  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
75  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
76  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
77  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
78 #endif
79 
80 
81  def->ltp_of_ecef.m[0] = -sin_lon;
82  def->ltp_of_ecef.m[1] = cos_lon;
83  def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
84  def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
85  def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
86  def->ltp_of_ecef.m[5] = cos_lat;
87  def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
88  def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
89  def->ltp_of_ecef.m[8] = sin_lat;
90 
91 }
92 
93 void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
94 
95  struct EcefCoor_i delta;
96  VECT3_DIFF(delta, *ecef, def->ecef);
97  const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*delta.x +
98  (int64_t)def->ltp_of_ecef.m[1]*delta.y +
99  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
100  enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
101  const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*delta.x +
102  (int64_t)def->ltp_of_ecef.m[4]*delta.y +
103  (int64_t)def->ltp_of_ecef.m[5]*delta.z;
104  enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
105  const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*delta.x +
106  (int64_t)def->ltp_of_ecef.m[7]*delta.y +
107  (int64_t)def->ltp_of_ecef.m[8]*delta.z;
108  enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
109 
110 }
111 
112 
113 void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
114  struct EnuCoor_i enu;
115  enu_of_ecef_point_i(&enu, def, ecef);
116  ENU_OF_TO_NED(*ned, enu);
117 }
118 
119 
125 void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
126  struct EnuCoor_i enu_cm;
127  enu_of_ecef_point_i(&enu_cm, def, ecef);
128 
129  /* enu = (enu_cm / 100) << INT32_POS_FRAC
130  * to loose less range:
131  * enu_cm = enu << (INT32_POS_FRAC-2) / 25
132  * which puts max enu output Q23.8 range to 8388km / 25 = 335km
133  */
134  INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC-2);
135  VECT3_SDIV(*enu, *enu, 25);
136 }
137 
138 
144 void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
145  struct EnuCoor_i enu;
146  enu_of_ecef_pos_i(&enu, def, ecef);
147  ENU_OF_TO_NED(*ned, enu);
148 }
149 
150 void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
151 
152  const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*ecef->x +
153  (int64_t)def->ltp_of_ecef.m[1]*ecef->y +
154  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
155  enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
156  const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*ecef->x +
157  (int64_t)def->ltp_of_ecef.m[4]*ecef->y +
158  (int64_t)def->ltp_of_ecef.m[5]*ecef->z;
159  enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
160  const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*ecef->x +
161  (int64_t)def->ltp_of_ecef.m[7]*ecef->y +
162  (int64_t)def->ltp_of_ecef.m[8]*ecef->z;
163  enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
164 
165 }
166 
167 
168 void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
169  struct EnuCoor_i enu;
170  enu_of_ecef_vect_i(&enu, def, ecef);
171  ENU_OF_TO_NED(*ned, enu);
172 }
173 
174 
175 void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
176 
177  const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0] * enu->x +
178  (int64_t)def->ltp_of_ecef.m[3] * enu->y +
179  (int64_t)def->ltp_of_ecef.m[6] * enu->z;
180  ecef->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
181 
182  const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[1] * enu->x +
183  (int64_t)def->ltp_of_ecef.m[4] * enu->y +
184  (int64_t)def->ltp_of_ecef.m[7] * enu->z;
185  ecef->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
186 
187  /* first element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ENU_to_ECEF */
188  const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[5] * enu->y +
189  (int64_t)def->ltp_of_ecef.m[8] * enu->z;
190  ecef->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
191 
192 }
193 
194 void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
195  struct EnuCoor_i enu;
196  ENU_OF_TO_NED(enu, *ned);
197  ecef_of_enu_vect_i(ecef, def, &enu);
198 }
199 
200 
206 void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
207  ecef_of_enu_vect_i(ecef, def, enu);
208  INT32_VECT3_ADD(*ecef, def->ecef);
209 }
210 
211 
217 void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
218  struct EnuCoor_i enu;
219  ENU_OF_TO_NED(enu, *ned);
220  ecef_of_enu_point_i(ecef, def, &enu);
221 }
222 
223 
229 void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
230  /* enu_cm = (enu * 100) >> INT32_POS_FRAC
231  * to loose less range:
232  * enu_cm = (enu * 25) >> (INT32_POS_FRAC-2)
233  * which puts max enu input Q23.8 range to 8388km / 25 = 335km
234  */
235  struct EnuCoor_i enu_cm;
236  VECT3_SMUL(enu_cm, *enu, 25);
237  INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC-2);
238  ecef_of_enu_vect_i(ecef, def, &enu_cm);
239  INT32_VECT3_ADD(*ecef, def->ecef);
240 }
241 
242 
248 void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
249  struct EnuCoor_i enu;
250  ENU_OF_TO_NED(enu, *ned);
251  ecef_of_enu_pos_i(ecef, def, &enu);
252 }
253 
254 
255 void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
256  struct EcefCoor_i ecef;
257  ecef_of_lla_i(&ecef,lla);
258  enu_of_ecef_point_i(enu,def,&ecef);
259 }
260 
261 void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
262  struct EcefCoor_i ecef;
263  ecef_of_lla_i(&ecef,lla);
264  ned_of_ecef_point_i(ned,def,&ecef);
265 }
266 
267 void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
268  struct EcefCoor_i ecef;
269  ecef_of_lla_i(&ecef,lla);
270  enu_of_ecef_vect_i(enu,def,&ecef);
271 }
272 
273 void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
274  struct EcefCoor_i ecef;
275  ecef_of_lla_i(&ecef,lla);
276  ned_of_ecef_vect_i(ned,def,&ecef);
277 }
278 
279 /*
280  For now we cheat and call the floating point version
281  Anyone up for writing it in fixed point ?
282 */
283 #include "pprz_geodetic_float.h"
284 #include "pprz_geodetic_double.h"
285 
286 void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) {
287 
288  /* convert our input to floating point */
289  struct EcefCoor_d in_d;
290  in_d.x = M_OF_CM((double)in->x);
291  in_d.y = M_OF_CM((double)in->y);
292  in_d.z = M_OF_CM((double)in->z);
293  /* calls the floating point transformation */
294  struct LlaCoor_d out_d;
295  lla_of_ecef_d(&out_d, &in_d);
296  /* convert the output to fixed point */
297  out->lon = (int32_t)rint(EM7RAD_OF_RAD(out_d.lon));
298  out->lat = (int32_t)rint(EM7RAD_OF_RAD(out_d.lat));
299  out->alt = (int32_t)MM_OF_M(out_d.alt);
300 
301 }
302 
303 void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) {
304 
305  /* convert our input to floating point */
306  struct LlaCoor_d in_d;
307  in_d.lon = RAD_OF_EM7RAD((double)in->lon);
308  in_d.lat = RAD_OF_EM7RAD((double)in->lat);
309  in_d.alt = M_OF_MM((double)in->alt);
310  /* calls the floating point transformation */
311  struct EcefCoor_d out_d;
312  ecef_of_lla_d(&out_d, &in_d);
313  /* convert the output to fixed point */
314  out->x = (int32_t)CM_OF_M(out_d.x);
315  out->y = (int32_t)CM_OF_M(out_d.y);
316  out->z = (int32_t)CM_OF_M(out_d.z);
317 
318 }
#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 radians*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)
#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:160
void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
#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)
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)
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 enu_of_ecef_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
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 EM7RAD_OF_RAD(_r)
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
double lat
in radians
int32_t lon
in radians*1e7
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:153
double z
in meters
struct EcefCoor_i ecef
Reference point in ecef.
#define RAD_OF_EM7RAD(_r)
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
#define M_OF_CM(_cm)
#define HIGH_RES_TRIG_FRAC
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:111
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:167
vector in Latitude, Longitude and Altitude
vector in East North Up coordinates
int32_t x
East.
Paparazzi fixed point algebra.
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:17