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_algebra_double.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_ALGEBRA_DOUBLE_H
31 #define PPRZ_ALGEBRA_DOUBLE_H
32 
33 #include "pprz_algebra.h"
34 #include "pprz_algebra_float.h"
35 
36 struct DoubleVect2 {
37  double x;
38  double y;
39 };
40 
41 struct DoubleVect3 {
42  double x;
43  double y;
44  double z;
45 };
46 
50 struct DoubleQuat {
51  double qi;
52  double qx;
53  double qy;
54  double qz;
55 };
56 
57 struct DoubleMat33 {
58  double m[3*3];
59 };
60 
64 struct DoubleRMat {
65  double m[3*3];
66 };
67 
71 struct DoubleEulers {
72  double phi;
73  double theta;
74  double psi;
75 };
76 
80 struct DoubleRates {
81  double p;
82  double q;
83  double r;
84 };
85 
86 #define DOUBLE_VECT3_ROUND(_v) DOUBLE_VECT3_RINT(_v, _v)
87 
88 
89 #define DOUBLE_VECT3_RINT(_vout, _vin) { \
90  (_vout).x = rint((_vin).x); \
91  (_vout).y = rint((_vin).y); \
92  (_vout).z = rint((_vin).z); \
93  }
94 
95 #define DOUBLE_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
96 
97 #define DOUBLE_VECT3_COPY(_a, _b) VECT3_COPY(_a, _b)
98 
99 #define DOUBLE_VECT3_SUM(_c,_a,_b) { \
100  (_c).x = (_a).x + (_b).x; \
101  (_c).y = (_a).y + (_b).y; \
102  (_c).z = (_a).z + (_b).z; \
103  }
104 
105 #define DOUBLE_VECT3_CROSS_PRODUCT(vo, v1, v2) FLOAT_VECT3_CROSS_PRODUCT(vo, v1, v2)
106 
107 #define DOUBLE_RMAT_OF_EULERS(_rm, _e) DOUBLE_RMAT_OF_EULERS_321(_rm, _e)
108 
109 #define DOUBLE_RMAT_OF_EULERS_321(_rm, _e) { \
110  \
111  const double sphi = sin((_e).phi); \
112  const double cphi = cos((_e).phi); \
113  const double stheta = sin((_e).theta); \
114  const double ctheta = cos((_e).theta); \
115  const double spsi = sin((_e).psi); \
116  const double cpsi = cos((_e).psi); \
117  \
118  RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi; \
119  RMAT_ELMT(_rm, 0, 1) = ctheta*spsi; \
120  RMAT_ELMT(_rm, 0, 2) = -stheta; \
121  RMAT_ELMT(_rm, 1, 0) = sphi*stheta*cpsi - cphi*spsi; \
122  RMAT_ELMT(_rm, 1, 1) = sphi*stheta*spsi + cphi*cpsi; \
123  RMAT_ELMT(_rm, 1, 2) = sphi*ctheta; \
124  RMAT_ELMT(_rm, 2, 0) = cphi*stheta*cpsi + sphi*spsi; \
125  RMAT_ELMT(_rm, 2, 1) = cphi*stheta*spsi - sphi*cpsi; \
126  RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
127  \
128  }
129 
130 
131 
132 
133 /* multiply _vin by _mat, store in _vout */
134 #define DOUBLE_MAT33_VECT3_MUL(_vout, _mat, _vin) { \
135  (_vout).x = (_mat)[0]*(_vin).x + (_mat)[1]*(_vin).y + (_mat)[2]*(_vin).z; \
136  (_vout).y = (_mat)[3]*(_vin).x + (_mat)[4]*(_vin).y + (_mat)[5]*(_vin).z; \
137  (_vout).z = (_mat)[6]*(_vin).x + (_mat)[7]*(_vin).y + (_mat)[8]*(_vin).z; \
138  }
139 
140 /* multiply _vin by the transpose of _mat, store in _vout */
141 #define DOUBLE_MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin) { \
142  (_vout).x = (_mat)[0]*(_vin).x + (_mat)[3]*(_vin).y + (_mat)[6]*(_vin).z; \
143  (_vout).y = (_mat)[1]*(_vin).x + (_mat)[4]*(_vin).y + (_mat)[7]*(_vin).z; \
144  (_vout).z = (_mat)[2]*(_vin).x + (_mat)[5]*(_vin).y + (_mat)[8]*(_vin).z; \
145  }
146 
147 #define DOUBLE_QUAT_OF_EULERS(_q, _e) { \
148  \
149  const double phi2 = (_e).phi/ 2.0; \
150  const double theta2 = (_e).theta/2.0; \
151  const double psi2 = (_e).psi/2.0; \
152  \
153  const double s_phi2 = sin( phi2 ); \
154  const double c_phi2 = cos( phi2 ); \
155  const double s_theta2 = sin( theta2 ); \
156  const double c_theta2 = cos( theta2 ); \
157  const double s_psi2 = sin( psi2 ); \
158  const double c_psi2 = cos( psi2 ); \
159  \
160  (_q).qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2; \
161  (_q).qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2; \
162  (_q).qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2; \
163  (_q).qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; \
164  \
165  }
166 
167 #define DOUBLE_EULERS_OF_QUAT(_e, _q) { \
168  \
169  const double qx2 = (_q).qx*(_q).qx; \
170  const double qy2 = (_q).qy*(_q).qy; \
171  const double qz2 = (_q).qz*(_q).qz; \
172  const double qiqx = (_q).qi*(_q).qx; \
173  const double qiqy = (_q).qi*(_q).qy; \
174  const double qiqz = (_q).qi*(_q).qz; \
175  const double qxqy = (_q).qx*(_q).qy; \
176  const double qxqz = (_q).qx*(_q).qz; \
177  const double qyqz = (_q).qy*(_q).qz; \
178  const double dcm00 = 1.0 - 2.*( qy2 + qz2 ); \
179  const double dcm01 = 2.*( qxqy + qiqz ); \
180  const double dcm02 = 2.*( qxqz - qiqy ); \
181  const double dcm12 = 2.*( qyqz + qiqx ); \
182  const double dcm22 = 1.0 - 2.*( qx2 + qy2 ); \
183  \
184  (_e).phi = atan2( dcm12, dcm22 ); \
185  (_e).theta = -asin( dcm02 ); \
186  (_e).psi = atan2( dcm01, dcm00 ); \
187  \
188  }
189 
190 #endif /* PPRZ_ALGEBRA_DOUBLE_H */
angular rates
double p
in rad/s^2
double phi
in radians
double m[3 *3]
euler angles
double m[3 *3]
Paparazzi floating point algebra.
double r
in rad/s^2
rotation matrix
double q
in rad/s^2
Roation quaternion.
double theta
in radians
double psi
in radians