Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
pprz_geodetic_int.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #include "pprz_geodetic_int.h"
25 #include "pprz_algebra_int.h"
26 
27 #define HIGH_RES_TRIG_FRAC 20
28 
29 void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
30 
31  /* store the origin of the tangeant plane */
32  VECT3_COPY(def->ecef, *ecef);
33  /* compute the lla representation of the origin */
34  lla_of_ecef_i(&def->lla, &def->ecef);
35  /* store the rotation matrix */
36 
37 #if 1
38  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
39  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
40  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
41  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
42 #else
43  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
44  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
45  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
46  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
47 #endif
48 
49 
50  def->ltp_of_ecef.m[0] = -sin_lon;
51  def->ltp_of_ecef.m[1] = cos_lon;
52  def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
53  def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
54  def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
55  def->ltp_of_ecef.m[5] = cos_lat;
56  def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
57  def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
58  def->ltp_of_ecef.m[8] = sin_lat;
59 
60 }
61 
62 void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla) {
63 
64  /* store the origin of the tangeant plane */
65  LLA_COPY(def->lla, *lla);
66  /* compute the ecef representation of the origin */
67  ecef_of_lla_i(&def->ecef, &def->lla);
68  /* store the rotation matrix */
69 
70 #if 1
71  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
72  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
73  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
74  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
75 #else
76  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
77  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
78  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
79  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
80 #endif
81 
82 
83  def->ltp_of_ecef.m[0] = -sin_lon;
84  def->ltp_of_ecef.m[1] = cos_lon;
85  def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
86  def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
87  def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
88  def->ltp_of_ecef.m[5] = cos_lat;
89  def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
90  def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
91  def->ltp_of_ecef.m[8] = sin_lat;
92 
93 }
94 
95 void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
96 
97  struct EcefCoor_i delta;
98  VECT3_DIFF(delta, *ecef, def->ecef);
99  const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*delta.x +
100  (int64_t)def->ltp_of_ecef.m[1]*delta.y +
101  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
102  enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
103  const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*delta.x +
104  (int64_t)def->ltp_of_ecef.m[4]*delta.y +
105  (int64_t)def->ltp_of_ecef.m[5]*delta.z;
106  enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
107  const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*delta.x +
108  (int64_t)def->ltp_of_ecef.m[7]*delta.y +
109  (int64_t)def->ltp_of_ecef.m[8]*delta.z;
110  enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
111 
112 }
113 
114 
115 void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
116 
117  struct EnuCoor_i enu;
118  enu_of_ecef_point_i(&enu, def, ecef);
119  ENU_OF_TO_NED(*ned, enu);
120 
121 }
122 
123 void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
124 
125  const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*ecef->x +
126  (int64_t)def->ltp_of_ecef.m[1]*ecef->y +
127  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
128  enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
129  const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*ecef->x +
130  (int64_t)def->ltp_of_ecef.m[4]*ecef->y +
131  (int64_t)def->ltp_of_ecef.m[5]*ecef->z;
132  enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
133  const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*ecef->x +
134  (int64_t)def->ltp_of_ecef.m[7]*ecef->y +
135  (int64_t)def->ltp_of_ecef.m[8]*ecef->z;
136  enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
137 
138 }
139 
140 
141 void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
142  struct EnuCoor_i enu;
143  enu_of_ecef_vect_i(&enu, def, ecef);
144  ENU_OF_TO_NED(*ned, enu);
145 }
146 
147 /* check if resolution of INT32_TRIG_FRAC (14) is enough here */
148 void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
149  INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);
150  INT32_VECT3_ADD(*ecef, def->ecef);
151 }
152 
153 void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
154  struct EnuCoor_i enu;
155  ENU_OF_TO_NED(enu, *ned);
156  ecef_of_enu_point_i(ecef, def, &enu);
157 }
158 
159 void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
160  INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);
161 }
162 
163 void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
164  struct EnuCoor_i enu;
165  ENU_OF_TO_NED(enu, *ned);
166  ecef_of_enu_vect_i(ecef, def, &enu);
167 }
168 
169 
170 void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
171  struct EcefCoor_i ecef;
172  ecef_of_lla_i(&ecef,lla);
173  enu_of_ecef_point_i(enu,def,&ecef);
174 }
175 
176 void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
177  struct EcefCoor_i ecef;
178  ecef_of_lla_i(&ecef,lla);
179  ned_of_ecef_point_i(ned,def,&ecef);
180 }
181 
182 void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
183  struct EcefCoor_i ecef;
184  ecef_of_lla_i(&ecef,lla);
185  enu_of_ecef_vect_i(enu,def,&ecef);
186 }
187 
188 void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
189  struct EcefCoor_i ecef;
190  ecef_of_lla_i(&ecef,lla);
191  ned_of_ecef_vect_i(ned,def,&ecef);
192 }
193 
194 /*
195  For now we cheat and call the floating point version
196  Anyone up for writing it in fixed point ?
197 */
198 #include "pprz_geodetic_float.h"
199 #include "pprz_geodetic_double.h"
200 
201 void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) {
202 
203  /* convert our input to floating point */
204  struct EcefCoor_d in_d;
205  in_d.x = M_OF_CM((double)in->x);
206  in_d.y = M_OF_CM((double)in->y);
207  in_d.z = M_OF_CM((double)in->z);
208  /* calls the floating point transformation */
209  struct LlaCoor_d out_d;
210  lla_of_ecef_d(&out_d, &in_d);
211  /* convert the output to fixed point */
212  out->lon = (int32_t)rint(EM7RAD_OF_RAD(out_d.lon));
213  out->lat = (int32_t)rint(EM7RAD_OF_RAD(out_d.lat));
214  out->alt = (int32_t)MM_OF_M(out_d.alt);
215 
216 }
217 
218 void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) {
219 
220  /* convert our input to floating point */
221  struct LlaCoor_d in_d;
222  in_d.lon = RAD_OF_EM7RAD((double)in->lon);
223  in_d.lat = RAD_OF_EM7RAD((double)in->lat);
224  in_d.alt = M_OF_MM((double)in->alt);
225  /* calls the floating point transformation */
226  struct EcefCoor_d out_d;
227  ecef_of_lla_d(&out_d, &in_d);
228  /* convert the output to fixed point */
229  out->x = (int32_t)CM_OF_M(out_d.x);
230  out->y = (int32_t)CM_OF_M(out_d.y);
231  out->z = (int32_t)CM_OF_M(out_d.z);
232 
233 }
#define INT32_VECT3_ADD(_a, _b)
int32_t y
in centimeters
int32_t lat
in radians*1e7
void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
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)
double x
in meters
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
#define M_OF_MM(_mm)
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
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
signed long long int64_t
Definition: types.h:21
#define CM_OF_M(_m)
int32_t z
in centimeters
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)
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)
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)
#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:155
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:113
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
int32_t m[3 *3]
vector in Latitude, Longitude and Altitude
vector in East North Up coordinates
Paparazzi fixed point algebra.
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:17