Paparazzi UAS  v7.0_unstable
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 {
54 };
55 
59 struct LlaCoor_i {
63 };
64 
68 struct NedCoor_i {
72 };
73 
77 struct EnuCoor_i {
81 };
82 
86 struct UtmCoor_i {
91 };
92 
98 struct LtpDef_i {
99  struct EcefCoor_i ecef;
100  struct LlaCoor_i lla;
101  struct Int32RMat ltp_of_ecef;
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 VECT3_CM_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 VECT3_FLOAT_OF_CM(_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 VECT3_DOUBLE_OF_CM(_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 
172 #define ECEF_BFP_OF_REAL(_o, _i) VECT3_CM_OF_REAL(_o, _i)
173 #define ECEF_FLOAT_OF_BFP(_o, _i) VECT3_FLOAT_OF_CM(_o, _i)
174 #define ECEF_DOUBLE_OF_BFP(_o, _i) VECT3_DOUBLE_OF_CM(_o, _i)
175 
176 #define LLA_BFP_OF_REAL(_o, _i) { \
177  (_o).lat = (int32_t)EM7DEG_OF_RAD((_i).lat); \
178  (_o).lon = (int32_t)EM7DEG_OF_RAD((_i).lon); \
179  (_o).alt = (int32_t)MM_OF_M((_i).alt); \
180  }
181 
182 #define LLA_FLOAT_OF_BFP(_o, _i) { \
183  (_o).lat = RAD_OF_EM7DEG((float)(_i).lat); \
184  (_o).lon = RAD_OF_EM7DEG((float)(_i).lon); \
185  (_o).alt = M_OF_MM((float)(_i).alt); \
186  }
187 
188 #define LLA_DOUBLE_OF_BFP(_o, _i) { \
189  (_o).lat = RAD_OF_EM7DEG((double)(_i).lat); \
190  (_o).lon = RAD_OF_EM7DEG((double)(_i).lon); \
191  (_o).alt = M_OF_MM((double)(_i).alt); \
192  }
193 #define NED_BFP_OF_REAL(_o, _i) { \
194  (_o).x = (int32_t)POS_BFP_OF_REAL((_i).x); \
195  (_o).y = (int32_t)POS_BFP_OF_REAL((_i).y); \
196  (_o).z = (int32_t)POS_BFP_OF_REAL((_i).z); \
197  }
198 
199 #define ENU_BFP_OF_REAL(_o, _i) NED_BFP_OF_REAL(_o, _i)
200 
201 #define NED_FLOAT_OF_BFP(_o, _i) { \
202  (_o).x = POS_FLOAT_OF_BFP((_i).x); \
203  (_o).y = POS_FLOAT_OF_BFP((_i).y); \
204  (_o).z = POS_FLOAT_OF_BFP((_i).z); \
205  }
206 
207 #define ENU_FLOAT_OF_BFP(_o, _i) NED_FLOAT_OF_BFP(_o, _i)
208 
209 #define INT32_VECT2_ENU_OF_NED(_o, _i) { \
210  (_o).x = (_i).y; \
211  (_o).y = (_i).x; \
212  }
213 
214 #define INT32_VECT2_NED_OF_ENU(_o, _i) INT32_VECT2_ENU_OF_NED(_o,_i)
215 
216 #define HIGH_RES_RMAT_BFP_OF_REAL(_ei, _ef) { \
217  (_ei).m[0] = BFP_OF_REAL((_ef).m[0], HIGH_RES_TRIG_FRAC); \
218  (_ei).m[1] = BFP_OF_REAL((_ef).m[1], HIGH_RES_TRIG_FRAC); \
219  (_ei).m[2] = BFP_OF_REAL((_ef).m[2], HIGH_RES_TRIG_FRAC); \
220  (_ei).m[3] = BFP_OF_REAL((_ef).m[3], HIGH_RES_TRIG_FRAC); \
221  (_ei).m[4] = BFP_OF_REAL((_ef).m[4], HIGH_RES_TRIG_FRAC); \
222  (_ei).m[5] = BFP_OF_REAL((_ef).m[5], HIGH_RES_TRIG_FRAC); \
223  (_ei).m[6] = BFP_OF_REAL((_ef).m[6], HIGH_RES_TRIG_FRAC); \
224  (_ei).m[7] = BFP_OF_REAL((_ef).m[7], HIGH_RES_TRIG_FRAC); \
225  (_ei).m[8] = BFP_OF_REAL((_ef).m[8], HIGH_RES_TRIG_FRAC); \
226  }
227 
228 #define HIGH_RES_RMAT_FLOAT_OF_BFP(_ef, _ei) { \
229  (_ef).m[0] = FLOAT_OF_BFP((_ei).m[0], HIGH_RES_TRIG_FRAC); \
230  (_ef).m[1] = FLOAT_OF_BFP((_ei).m[1], HIGH_RES_TRIG_FRAC); \
231  (_ef).m[2] = FLOAT_OF_BFP((_ei).m[2], HIGH_RES_TRIG_FRAC); \
232  (_ef).m[3] = FLOAT_OF_BFP((_ei).m[3], HIGH_RES_TRIG_FRAC); \
233  (_ef).m[4] = FLOAT_OF_BFP((_ei).m[4], HIGH_RES_TRIG_FRAC); \
234  (_ef).m[5] = FLOAT_OF_BFP((_ei).m[5], HIGH_RES_TRIG_FRAC); \
235  (_ef).m[6] = FLOAT_OF_BFP((_ei).m[6], HIGH_RES_TRIG_FRAC); \
236  (_ef).m[7] = FLOAT_OF_BFP((_ei).m[7], HIGH_RES_TRIG_FRAC); \
237  (_ef).m[8] = FLOAT_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \
238  }
239 
240 #define HIGH_RES_RMAT_DOUBLE_OF_BFP(_ef, _ei) { \
241  (_ef).m[0] = DOUBLE_OF_BFP((_ei).m[0], HIGH_RES_TRIG_FRAC); \
242  (_ef).m[1] = DOUBLE_OF_BFP((_ei).m[1], HIGH_RES_TRIG_FRAC); \
243  (_ef).m[2] = DOUBLE_OF_BFP((_ei).m[2], HIGH_RES_TRIG_FRAC); \
244  (_ef).m[3] = DOUBLE_OF_BFP((_ei).m[3], HIGH_RES_TRIG_FRAC); \
245  (_ef).m[4] = DOUBLE_OF_BFP((_ei).m[4], HIGH_RES_TRIG_FRAC); \
246  (_ef).m[5] = DOUBLE_OF_BFP((_ei).m[5], HIGH_RES_TRIG_FRAC); \
247  (_ef).m[6] = DOUBLE_OF_BFP((_ei).m[6], HIGH_RES_TRIG_FRAC); \
248  (_ef).m[7] = DOUBLE_OF_BFP((_ei).m[7], HIGH_RES_TRIG_FRAC); \
249  (_ef).m[8] = DOUBLE_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \
250  }
251 
252 #define UTM_FLOAT_OF_BFP(_o, _i) { \
253  (_o).east = M_OF_CM((float)(_i).east); \
254  (_o).north = M_OF_CM((float)(_i).north); \
255  (_o).alt = M_OF_MM((float)(_i).alt); \
256  (_o).zone = (_i).zone; \
257  }
258 
259 #define UTM_DOUBLE_OF_BFP(_o, _i) { \
260  (_o).east = M_OF_CM((double)(_i).east); \
261  (_o).north = M_OF_CM((double)(_i).north); \
262  (_o).alt = M_OF_MM((double)(_i).alt); \
263  (_o).zone = (_i).zone; \
264  }
265 
266 #define UTM_BFP_OF_REAL(_o, _i) { \
267  (_o).east = (int32_t)CM_OF_M((_i).east); \
268  (_o).north = (int32_t)CM_OF_M((_i).north); \
269  (_o).alt = (int32_t)MM_OF_M((_i).alt); \
270  (_o).zone = (_i).zone; \
271  }
272 
273 #ifdef __cplusplus
274 } /* extern "C" */
275 #endif
276 
277 #endif /* PPRZ_GEODETIC_INT_H */
rotation matrix
int32_t lat
in degrees*1e7
int32_t y
North.
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
struct Int32RMat ltp_of_ecef
Rotation matrix.
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t z
Up.
int32_t z
in centimeters
uint8_t zone
UTM zone number.
struct LlaCoor_i lla
Reference point in lla.
int32_t x
in centimeters
int32_t y
East.
struct EcefCoor_i ecef
Reference point in ecef.
int32_t east
in centimeters
int32_t y
in centimeters
int32_t x
East.
int32_t lon
in degrees*1e7
int32_t x
North.
int32_t north
in centimeters
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.
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm)
Convert a UTM to LLA.
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.
void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
void ltp_def_from_ecef_i(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)
Convert a point from LLA to local NED.
void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla)
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.
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.
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 ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Rotate a vector from ENU to ECEF.
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.
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.
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.
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 utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
Convert a LLA to UTM.
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.
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.
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.
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *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.
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.
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
position in UTM coordinates
Paparazzi fixed point algebra.
Paparazzi generic macros for geodetic calculations.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98