-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
pprz_geodetic_int.h
125 lines (103 loc) · 4.2 KB
/
pprz_geodetic_int.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
#ifndef PPRZ_GEODETIC_INT_H
#define PPRZ_GEODETIC_INT_H
#include "pprz_geodetic.h"
#include "std.h"
#include "pprz_algebra_int.h"
/*
Earth Centered Earth Fixed in centimeters
*/
struct EcefCoor_i {
int32_t x;
int32_t y;
int32_t z;
};
/* lon, lat in radians*1e7 */
/* alt in centimeters */
struct LlaCoor_i {
int32_t lon;
int32_t lat;
int32_t alt;
};
/* North East Down local tangeant plane */
struct NedCoor_i {
int32_t x;
int32_t y;
int32_t z;
};
/* East North Up local tangeant plane */
struct EnuCoor_i {
int32_t x;
int32_t y;
int32_t z;
};
/* Local tangeant plane definition */
struct LtpDef_i {
struct EcefCoor_i ecef; /* Reference point in ecef */
struct LlaCoor_i lla; /* Reference point in lla */
struct Int32Mat33 ltp_of_ecef; /* Rotation matrix */
int32_t hmsl; /* Height above mean sea level */
};
extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in);
extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in);
extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
#define CM_OF_M(_m) ((_m)*1e2)
#define M_OF_CM(_cm) ((_cm)/1e2)
#define EM7RAD_OF_RAD(_r) (_r*1e7)
#define RAD_OF_EM7RAD(_r) (_r/1e7)
#define INT32_VECT3_ENU_OF_NED(_o, _i) { \
(_o).x = (_i).y; \
(_o).y = (_i).x; \
(_o).z = -(_i).z; \
}
#define INT32_VECT3_NED_OF_ENU(_o, _i) INT32_VECT3_ENU_OF_NED(_o,_i)
#define ECEF_BFP_OF_REAL(_o, _i) { \
(_o).x = (int32_t)CM_OF_M((_i).x); \
(_o).y = (int32_t)CM_OF_M((_i).y); \
(_o).z = (int32_t)CM_OF_M((_i).z); \
}
#define ECEF_FLOAT_OF_BFP(_o, _i) { \
(_o).x = (float)M_OF_CM((_i).x); \
(_o).y = (float)M_OF_CM((_i).y); \
(_o).z = (float)M_OF_CM((_i).z); \
}
#define LLA_BFP_OF_REAL(_o, _i) { \
(_o).lat = (int32_t)EM7RAD_OF_RAD((_i).lat); \
(_o).lon = (int32_t)EM7RAD_OF_RAD((_i).lon); \
(_o).alt = (int32_t)CM_OF_M((_i).alt); \
}
#define LLA_FLOAT_OF_BFP(_o, _i) { \
(_o).lat = (float)RAD_OF_EM7RAD((_i).lat); \
(_o).lon = (float)RAD_OF_EM7RAD((_i).lon); \
(_o).alt = (float)M_OF_CM((_i).alt); \
}
#define NED_BFP_OF_REAL(_o, _i) { \
(_o).x = POS_BFP_OF_REAL((_i).x); \
(_o).y = POS_BFP_OF_REAL((_i).y); \
(_o).z = POS_BFP_OF_REAL((_i).z); \
}
#define ENU_BFP_OF_REAL(_o, _i) NED_BFP_OF_REAL(_o, _i)
#define NED_FLOAT_OF_BFP(_o, _i) { \
(_o).x = POS_FLOAT_OF_BFP((_i).x); \
(_o).y = POS_FLOAT_OF_BFP((_i).y); \
(_o).z = POS_FLOAT_OF_BFP((_i).z); \
}
#define ENU_FLOAT_OF_BFP(_o, _i) NED_FLOAT_OF_BFP(_o, _i)
#define INT32_VECT2_ENU_OF_NED(_o, _i) { \
(_o).x = (_i).y; \
(_o).y = (_i).x; \
}
#define INT32_VECT2_NED_OF_ENU(_o, _i) INT32_VECT2_ENU_OF_NED(_o,_i)
#endif /* PPRZ_GEODETIC_INT_H */