Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_geodetic_int.h
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 
31 #ifndef PPRZ_GEODETIC_INT_H
32 #define PPRZ_GEODETIC_INT_H
33 
34 #ifdef __cplusplus
35 extern "C" {
36 #endif
37 
38 #include "pprz_geodetic.h"
39 
40 #include "std.h"
41 #include "pprz_algebra_int.h"
42 
43 
50 struct EcefCoor_i {
51  int32_t x;
52  int32_t y;
53  int32_t z;
54 };
55 
59 struct LlaCoor_i {
60  int32_t lat;
61  int32_t lon;
62  int32_t alt;
63 };
64 
68 struct NedCoor_i {
69  int32_t x;
70  int32_t y;
71  int32_t z;
72 };
73 
77 struct EnuCoor_i {
78  int32_t x;
79  int32_t y;
80  int32_t z;
81 };
82 
86 struct UtmCoor_i {
87  int32_t north;
88  int32_t east;
89  int32_t alt;
90  uint8_t zone;
91 };
92 
98 struct LtpDef_i {
99  struct EcefCoor_i ecef;
100  struct LlaCoor_i lla;
101  struct Int32RMat ltp_of_ecef;
102  int32_t hmsl;
103 };
104 
105 extern void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm);
106 extern void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla);
107 extern void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla);
108 extern void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef);
109 extern void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla);
110 extern void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in);
111 extern void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in);
112 extern void enu_of_ecef_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef);
113 extern void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef);
114 extern void enu_of_ecef_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef);
115 extern void ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef);
116 extern void enu_of_ecef_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef);
117 extern void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef);
118 extern void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla);
119 extern void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla);
120 extern void enu_of_lla_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla);
121 extern void ned_of_lla_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla);
122 extern void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla);
123 extern void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla);
124 extern void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu);
125 extern void ecef_of_ned_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned);
126 extern void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu);
127 extern void ecef_of_ned_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned);
128 extern void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu);
129 extern void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned);
130 
131 #define CM_OF_M(_m) ((_m)*1e2)
132 #define M_OF_CM(_cm) ((_cm)/1e2)
133 #define MM_OF_M(_m) ((_m)*1e3)
134 #define M_OF_MM(_mm) ((_mm)/1e3)
135 #define EM7RAD_OF_RAD(_r) ((_r)*1e7)
136 #define RAD_OF_EM7RAD(_r) ((_r)/1e7)
137 #define EM7DEG_OF_DEG(_r) ((_r)*1e7)
138 #define DEG_OF_EM7DEG(_r) ((_r)/1e7)
139 #define EM7DEG_OF_RAD(_r) (DegOfRad(_r)*1e7)
140 #define RAD_OF_EM7DEG(_r) (RadOfDeg(_r)/1e7)
141 
142 #define HIGH_RES_TRIG_FRAC 20
143 
144 #define VECT3_ENU_OF_NED(_o, _i) { \
145  (_o).x = (_i).y; \
146  (_o).y = (_i).x; \
147  (_o).z = -(_i).z; \
148  }
149 
150 #define VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
151 #define INT32_VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
152 #define INT32_VECT3_ENU_OF_NED(_o, _i) VECT3_ENU_OF_NED(_o,_i)
153 
154 #define ECEF_BFP_OF_REAL(_o, _i) { \
155  (_o).x = (int32_t)CM_OF_M((_i).x); \
156  (_o).y = (int32_t)CM_OF_M((_i).y); \
157  (_o).z = (int32_t)CM_OF_M((_i).z); \
158  }
159 
160 #define ECEF_FLOAT_OF_BFP(_o, _i) { \
161  (_o).x = M_OF_CM((float)(_i).x); \
162  (_o).y = M_OF_CM((float)(_i).y); \
163  (_o).z = M_OF_CM((float)(_i).z); \
164  }
165 
166 #define ECEF_DOUBLE_OF_BFP(_o, _i) { \
167  (_o).x = M_OF_CM((double)(_i).x); \
168  (_o).y = M_OF_CM((double)(_i).y); \
169  (_o).z = M_OF_CM((double)(_i).z); \
170  }
171 #define LLA_BFP_OF_REAL(_o, _i) { \
172  (_o).lat = (int32_t)EM7DEG_OF_RAD((_i).lat); \
173  (_o).lon = (int32_t)EM7DEG_OF_RAD((_i).lon); \
174  (_o).alt = (int32_t)MM_OF_M((_i).alt); \
175  }
176 
177 #define LLA_FLOAT_OF_BFP(_o, _i) { \
178  (_o).lat = RAD_OF_EM7DEG((float)(_i).lat); \
179  (_o).lon = RAD_OF_EM7DEG((float)(_i).lon); \
180  (_o).alt = M_OF_MM((float)(_i).alt); \
181  }
182 
183 #define LLA_DOUBLE_OF_BFP(_o, _i) { \
184  (_o).lat = RAD_OF_EM7DEG((double)(_i).lat); \
185  (_o).lon = RAD_OF_EM7DEG((double)(_i).lon); \
186  (_o).alt = M_OF_MM((double)(_i).alt); \
187  }
188 #define NED_BFP_OF_REAL(_o, _i) { \
189  (_o).x = (int32_t)POS_BFP_OF_REAL((_i).x); \
190  (_o).y = (int32_t)POS_BFP_OF_REAL((_i).y); \
191  (_o).z = (int32_t)POS_BFP_OF_REAL((_i).z); \
192  }
193 
194 #define ENU_BFP_OF_REAL(_o, _i) NED_BFP_OF_REAL(_o, _i)
195 
196 #define NED_FLOAT_OF_BFP(_o, _i) { \
197  (_o).x = POS_FLOAT_OF_BFP((_i).x); \
198  (_o).y = POS_FLOAT_OF_BFP((_i).y); \
199  (_o).z = POS_FLOAT_OF_BFP((_i).z); \
200  }
201 
202 #define ENU_FLOAT_OF_BFP(_o, _i) NED_FLOAT_OF_BFP(_o, _i)
203 
204 #define INT32_VECT2_ENU_OF_NED(_o, _i) { \
205  (_o).x = (_i).y; \
206  (_o).y = (_i).x; \
207  }
208 
209 #define INT32_VECT2_NED_OF_ENU(_o, _i) INT32_VECT2_ENU_OF_NED(_o,_i)
210 
211 #define HIGH_RES_RMAT_BFP_OF_REAL(_ei, _ef) { \
212  (_ei).m[0] = BFP_OF_REAL((_ef).m[0], HIGH_RES_TRIG_FRAC); \
213  (_ei).m[1] = BFP_OF_REAL((_ef).m[1], HIGH_RES_TRIG_FRAC); \
214  (_ei).m[2] = BFP_OF_REAL((_ef).m[2], HIGH_RES_TRIG_FRAC); \
215  (_ei).m[3] = BFP_OF_REAL((_ef).m[3], HIGH_RES_TRIG_FRAC); \
216  (_ei).m[4] = BFP_OF_REAL((_ef).m[4], HIGH_RES_TRIG_FRAC); \
217  (_ei).m[5] = BFP_OF_REAL((_ef).m[5], HIGH_RES_TRIG_FRAC); \
218  (_ei).m[6] = BFP_OF_REAL((_ef).m[6], HIGH_RES_TRIG_FRAC); \
219  (_ei).m[7] = BFP_OF_REAL((_ef).m[7], HIGH_RES_TRIG_FRAC); \
220  (_ei).m[8] = BFP_OF_REAL((_ef).m[8], HIGH_RES_TRIG_FRAC); \
221  }
222 
223 #define HIGH_RES_RMAT_FLOAT_OF_BFP(_ef, _ei) { \
224  (_ef).m[0] = FLOAT_OF_BFP((_ei).m[0], HIGH_RES_TRIG_FRAC); \
225  (_ef).m[1] = FLOAT_OF_BFP((_ei).m[1], HIGH_RES_TRIG_FRAC); \
226  (_ef).m[2] = FLOAT_OF_BFP((_ei).m[2], HIGH_RES_TRIG_FRAC); \
227  (_ef).m[3] = FLOAT_OF_BFP((_ei).m[3], HIGH_RES_TRIG_FRAC); \
228  (_ef).m[4] = FLOAT_OF_BFP((_ei).m[4], HIGH_RES_TRIG_FRAC); \
229  (_ef).m[5] = FLOAT_OF_BFP((_ei).m[5], HIGH_RES_TRIG_FRAC); \
230  (_ef).m[6] = FLOAT_OF_BFP((_ei).m[6], HIGH_RES_TRIG_FRAC); \
231  (_ef).m[7] = FLOAT_OF_BFP((_ei).m[7], HIGH_RES_TRIG_FRAC); \
232  (_ef).m[8] = FLOAT_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \
233  }
234 
235 #define HIGH_RES_RMAT_DOUBLE_OF_BFP(_ef, _ei) { \
236  (_ef).m[0] = DOUBLE_OF_BFP((_ei).m[0], HIGH_RES_TRIG_FRAC); \
237  (_ef).m[1] = DOUBLE_OF_BFP((_ei).m[1], HIGH_RES_TRIG_FRAC); \
238  (_ef).m[2] = DOUBLE_OF_BFP((_ei).m[2], HIGH_RES_TRIG_FRAC); \
239  (_ef).m[3] = DOUBLE_OF_BFP((_ei).m[3], HIGH_RES_TRIG_FRAC); \
240  (_ef).m[4] = DOUBLE_OF_BFP((_ei).m[4], HIGH_RES_TRIG_FRAC); \
241  (_ef).m[5] = DOUBLE_OF_BFP((_ei).m[5], HIGH_RES_TRIG_FRAC); \
242  (_ef).m[6] = DOUBLE_OF_BFP((_ei).m[6], HIGH_RES_TRIG_FRAC); \
243  (_ef).m[7] = DOUBLE_OF_BFP((_ei).m[7], HIGH_RES_TRIG_FRAC); \
244  (_ef).m[8] = DOUBLE_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \
245  }
246 
247 #define UTM_FLOAT_OF_BFP(_o, _i) { \
248  (_o).east = M_OF_CM((float)(_i).east); \
249  (_o).north = M_OF_CM((float)(_i).north); \
250  (_o).alt = M_OF_MM((float)(_i).alt); \
251  (_o).zone = (_i).zone; \
252  }
253 
254 #define UTM_DOUBLE_OF_BFP(_o, _i) { \
255  (_o).east = M_OF_CM((double)(_i).east); \
256  (_o).north = M_OF_CM((double)(_i).north); \
257  (_o).alt = M_OF_MM((double)(_i).alt); \
258  (_o).zone = (_i).zone; \
259  }
260 
261 #define UTM_BFP_OF_REAL(_o, _i) { \
262  (_o).east = (int32_t)CM_OF_M((_i).east); \
263  (_o).north = (int32_t)CM_OF_M((_i).north); \
264  (_o).alt = (int32_t)MM_OF_M((_i).alt); \
265  (_o).zone = (_i).zone; \
266  }
267 
268 #ifdef __cplusplus
269 } /* extern "C" */
270 #endif
271 
272 #endif /* PPRZ_GEODETIC_INT_H */
273 
lla_of_utm_i
void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm)
Convert a UTM to LLA.
Definition: pprz_geodetic_int.c:452
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
pprz_geodetic.h
Paparazzi generic macros for geodetic calculations.
UtmCoor_i::north
int32_t north
in centimeters
Definition: pprz_geodetic_int.h:87
ltp_def_from_ecef_i
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
Definition: pprz_geodetic_int.c:60
EnuCoor_i::y
int32_t y
North.
Definition: pprz_geodetic_int.h:79
lla_of_ecef_i
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
Definition: pprz_geodetic_int.c:363
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
EnuCoor_i::x
int32_t x
East.
Definition: pprz_geodetic_int.h:78
utm_of_lla_i
void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
Convert a LLA to UTM.
Definition: pprz_geodetic_int.c:423
LtpDef_i
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_int.h:98
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
ltp_of_ecef_rmat_from_lla_i
void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:34
ecef_of_lla_i
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
Definition: pprz_geodetic_int.c:392
ecef_of_enu_vect_i
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.
Definition: pprz_geodetic_int.c:199
ecef_of_ned_point_i
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.
Definition: pprz_geodetic_int.c:250
ned_of_lla_point_i
void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
Definition: pprz_geodetic_int.c:307
pprz_algebra_int.h
Paparazzi fixed point algebra.
ned_of_lla_vect_i
void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:345
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
UtmCoor_i::east
int32_t east
in centimeters
Definition: pprz_geodetic_int.h:88
std.h
enu_of_ecef_point_i
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.
Definition: pprz_geodetic_int.c:90
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
ecef_of_enu_pos_i
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.
Definition: pprz_geodetic_int.c:263
enu_of_lla_point_i
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
Definition: pprz_geodetic_int.c:295
EcefCoor_i
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_int.h:50
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
UtmCoor_i::alt
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_int.h:89
uint8_t
unsigned char uint8_t
Definition: types.h:14
enu_of_lla_pos_i
void enu_of_lla_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
Definition: pprz_geodetic_int.c:319
enu_of_ecef_vect_i
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.
Definition: pprz_geodetic_int.c:162
ned_of_ecef_pos_i
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.
Definition: pprz_geodetic_int.c:149
ned_of_ecef_vect_i
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.
Definition: pprz_geodetic_int.c:186
ltp_def_from_lla_i
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:72
ecef_of_ned_vect_i
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.
Definition: pprz_geodetic_int.c:225
LtpDef_i::ltp_of_ecef
struct Int32RMat ltp_of_ecef
Rotation matrix.
Definition: pprz_geodetic_int.h:101
enu_of_lla_vect_i
void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:338
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
int32_t
signed long int32_t
Definition: types.h:19
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
ecef_of_enu_point_i
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.
Definition: pprz_geodetic_int.c:238
UtmCoor_i
position in UTM coordinates
Definition: pprz_geodetic_int.h:86
ned_of_ecef_point_i
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.
Definition: pprz_geodetic_int.c:116
EnuCoor_i::z
int32_t z
Up.
Definition: pprz_geodetic_int.h:80
enu_of_ecef_pos_i
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.
Definition: pprz_geodetic_int.c:129
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
EnuCoor_i
vector in East North Up coordinates
Definition: pprz_geodetic_int.h:77
ecef_of_ned_pos_i
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.
Definition: pprz_geodetic_int.c:283
UtmCoor_i::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_int.h:90
ned_of_lla_pos_i
void ned_of_lla_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
Definition: pprz_geodetic_int.c:331