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.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2011 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  */
21 
30 #ifndef PPRZ_GEODETIC_INT_H
31 #define PPRZ_GEODETIC_INT_H
32 
33 #include "pprz_geodetic.h"
34 
35 #include "std.h"
36 #include "pprz_algebra_int.h"
37 
38 
45 struct EcefCoor_i {
49 };
50 
54 struct LlaCoor_i {
58 };
59 
63 struct NedCoor_i {
67 };
68 
72 struct EnuCoor_i {
76 };
77 
81 struct UtmCoor_i {
86 };
87 
93 struct LtpDef_i {
94  struct EcefCoor_i ecef;
95  struct LlaCoor_i lla;
98 };
99 
100 extern void ltp_of_ecef_rmat_from_lla_i(struct Int32Mat33* ltp_of_ecef, struct LlaCoor_i* lla);
101 extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef);
102 extern void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla);
103 extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in);
104 extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in);
105 extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
106 extern void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
107 extern void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
108 extern void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
109 extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
110 extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
111 extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
112 extern void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
113 extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
114 extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
115 extern void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
116 extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
117 extern void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
118 extern void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
119 extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
120 extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
121 
122 #define CM_OF_M(_m) ((_m)*1e2)
123 #define M_OF_CM(_cm) ((_cm)/1e2)
124 #define MM_OF_M(_m) ((_m)*1e3)
125 #define M_OF_MM(_mm) ((_mm)/1e3)
126 #define EM7RAD_OF_RAD(_r) ((_r)*1e7)
127 #define RAD_OF_EM7RAD(_r) ((_r)/1e7)
128 #define EM7DEG_OF_RAD(_r) (DegOfRad(_r)*1e7)
129 #define RAD_OF_EM7DEG(_r) (RadOfDeg(_r)/1e7)
130 
131 #define HIGH_RES_TRIG_FRAC 20
132 
133 #define VECT3_ENU_OF_NED(_o, _i) { \
134  (_o).x = (_i).y; \
135  (_o).y = (_i).x; \
136  (_o).z = -(_i).z; \
137  }
138 
139 #define VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
140 #define INT32_VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
141 #define INT32_VECT3_ENU_OF_NED(_o, _i) VECT3_ENU_OF_NED(_o,_i)
142 
143 #define ECEF_BFP_OF_REAL(_o, _i) { \
144  (_o).x = (int32_t)CM_OF_M((_i).x); \
145  (_o).y = (int32_t)CM_OF_M((_i).y); \
146  (_o).z = (int32_t)CM_OF_M((_i).z); \
147  }
148 
149 #define ECEF_FLOAT_OF_BFP(_o, _i) { \
150  (_o).x = (float)M_OF_CM((_i).x); \
151  (_o).y = (float)M_OF_CM((_i).y); \
152  (_o).z = (float)M_OF_CM((_i).z); \
153  }
154 
155 #define ECEF_DOUBLE_OF_BFP(_o, _i) { \
156  (_o).x = (double)M_OF_CM((_i).x); \
157  (_o).y = (double)M_OF_CM((_i).y); \
158  (_o).z = (double)M_OF_CM((_i).z); \
159  }
160 #define LLA_BFP_OF_REAL(_o, _i) { \
161  (_o).lat = (int32_t)EM7DEG_OF_RAD((_i).lat); \
162  (_o).lon = (int32_t)EM7DEG_OF_RAD((_i).lon); \
163  (_o).alt = (int32_t)MM_OF_M((_i).alt); \
164  }
165 
166 #define LLA_FLOAT_OF_BFP(_o, _i) { \
167  (_o).lat = RAD_OF_EM7DEG((float)(_i).lat); \
168  (_o).lon = RAD_OF_EM7DEG((float)(_i).lon); \
169  (_o).alt = M_OF_MM((float)(_i).alt); \
170  }
171 
172 #define LLA_DOUBLE_OF_BFP(_o, _i) { \
173  (_o).lat = RAD_OF_EM7DEG((double)(_i).lat); \
174  (_o).lon = RAD_OF_EM7DEG((double)(_i).lon); \
175  (_o).alt = M_OF_MM((double)(_i).alt); \
176  }
177 #define NED_BFP_OF_REAL(_o, _i) { \
178  (_o).x = POS_BFP_OF_REAL((_i).x); \
179  (_o).y = POS_BFP_OF_REAL((_i).y); \
180  (_o).z = POS_BFP_OF_REAL((_i).z); \
181  }
182 
183 #define ENU_BFP_OF_REAL(_o, _i) NED_BFP_OF_REAL(_o, _i)
184 
185 #define NED_FLOAT_OF_BFP(_o, _i) { \
186  (_o).x = POS_FLOAT_OF_BFP((_i).x); \
187  (_o).y = POS_FLOAT_OF_BFP((_i).y); \
188  (_o).z = POS_FLOAT_OF_BFP((_i).z); \
189  }
190 
191 #define ENU_FLOAT_OF_BFP(_o, _i) NED_FLOAT_OF_BFP(_o, _i)
192 
193 #define INT32_VECT2_ENU_OF_NED(_o, _i) { \
194  (_o).x = (_i).y; \
195  (_o).y = (_i).x; \
196  }
197 
198 #define INT32_VECT2_NED_OF_ENU(_o, _i) INT32_VECT2_ENU_OF_NED(_o,_i)
199 
200 #define HIGH_RES_RMAT_BFP_OF_REAL(_ei, _ef) { \
201  (_ei).m[0] = BFP_OF_REAL((_ef).m[0], HIGH_RES_TRIG_FRAC); \
202  (_ei).m[1] = BFP_OF_REAL((_ef).m[1], HIGH_RES_TRIG_FRAC); \
203  (_ei).m[2] = BFP_OF_REAL((_ef).m[2], HIGH_RES_TRIG_FRAC); \
204  (_ei).m[3] = BFP_OF_REAL((_ef).m[3], HIGH_RES_TRIG_FRAC); \
205  (_ei).m[4] = BFP_OF_REAL((_ef).m[4], HIGH_RES_TRIG_FRAC); \
206  (_ei).m[5] = BFP_OF_REAL((_ef).m[5], HIGH_RES_TRIG_FRAC); \
207  (_ei).m[6] = BFP_OF_REAL((_ef).m[6], HIGH_RES_TRIG_FRAC); \
208  (_ei).m[7] = BFP_OF_REAL((_ef).m[7], HIGH_RES_TRIG_FRAC); \
209  (_ei).m[8] = BFP_OF_REAL((_ef).m[8], HIGH_RES_TRIG_FRAC); \
210  }
211 
212 #define HIGH_RES_RMAT_FLOAT_OF_BFP(_ef, _ei) { \
213  (_ef).m[0] = FLOAT_OF_BFP((_ei).m[0], HIGH_RES_TRIG_FRAC); \
214  (_ef).m[1] = FLOAT_OF_BFP((_ei).m[1], HIGH_RES_TRIG_FRAC); \
215  (_ef).m[2] = FLOAT_OF_BFP((_ei).m[2], HIGH_RES_TRIG_FRAC); \
216  (_ef).m[3] = FLOAT_OF_BFP((_ei).m[3], HIGH_RES_TRIG_FRAC); \
217  (_ef).m[4] = FLOAT_OF_BFP((_ei).m[4], HIGH_RES_TRIG_FRAC); \
218  (_ef).m[5] = FLOAT_OF_BFP((_ei).m[5], HIGH_RES_TRIG_FRAC); \
219  (_ef).m[6] = FLOAT_OF_BFP((_ei).m[6], HIGH_RES_TRIG_FRAC); \
220  (_ef).m[7] = FLOAT_OF_BFP((_ei).m[7], HIGH_RES_TRIG_FRAC); \
221  (_ef).m[8] = FLOAT_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \
222  }
223 
224 #define HIGH_RES_RMAT_DOUBLE_OF_BFP(_ef, _ei) { \
225  (_ef).m[0] = DOUBLE_OF_BFP((_ei).m[0], HIGH_RES_TRIG_FRAC); \
226  (_ef).m[1] = DOUBLE_OF_BFP((_ei).m[1], HIGH_RES_TRIG_FRAC); \
227  (_ef).m[2] = DOUBLE_OF_BFP((_ei).m[2], HIGH_RES_TRIG_FRAC); \
228  (_ef).m[3] = DOUBLE_OF_BFP((_ei).m[3], HIGH_RES_TRIG_FRAC); \
229  (_ef).m[4] = DOUBLE_OF_BFP((_ei).m[4], HIGH_RES_TRIG_FRAC); \
230  (_ef).m[5] = DOUBLE_OF_BFP((_ei).m[5], HIGH_RES_TRIG_FRAC); \
231  (_ef).m[6] = DOUBLE_OF_BFP((_ei).m[6], HIGH_RES_TRIG_FRAC); \
232  (_ef).m[7] = DOUBLE_OF_BFP((_ei).m[7], HIGH_RES_TRIG_FRAC); \
233  (_ef).m[8] = DOUBLE_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \
234  }
235 
236 #endif /* PPRZ_GEODETIC_INT_H */
int32_t y
in centimeters
int32_t lat
in degrees*1e7
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.
struct Int32Mat33 ltp_of_ecef
Rotation matrix.
vector in EarthCenteredEarthFixed coordinates
uint8_t zone
UTM zone number.
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 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 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.
int32_t x
North.
struct LlaCoor_i lla
Reference point in lla.
vector in Latitude, Longitude and Altitude
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
int32_t z
in centimeters
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Up.
vector in North East Down coordinates
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
int32_t z
Down.
definition of the local (flat earth) coordinate system
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 ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
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 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 alt
in millimeters above WGS84 reference ellipsoid
int32_t y
East.
void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
int32_t north
in centimeters
int32_t x
in centimeters
position in UTM coordinates
signed long int32_t
Definition: types.h:19
int32_t east
in centimeters
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in degrees*1e7
int32_t hmsl
Height above mean sea level in 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 EcefCoor_i ecef
Reference point in ecef.
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 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 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 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 ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
int32_t y
North.
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in East North Up coordinates
int32_t x
East.
Paparazzi fixed point algebra.