Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_algebra_int.c
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 
27 #include "pprz_algebra_int.h"
28 
29 #define INT32_SQRT_MAX_ITER 40
30 uint32_t int32_sqrt(uint32_t in)
31 {
32  if (in == 0) {
33  return 0;
34  } else {
35  uint32_t s1, s2;
36  uint8_t iter = 0;
37  s2 = in;
38  do {
39  s1 = s2;
40  s2 = in / s1;
41  s2 += s1;
42  s2 /= 2;
43  iter++;
44  } while (((s1 - s2) > 1) && (iter < INT32_SQRT_MAX_ITER));
45  return s2;
46  }
47 }
48 
49 
50 /*
51  *
52  * Rotation matrices
53  *
54  */
55 
59 void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c)
60 {
61  m_a2c->m[0] = (m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6]) >> INT32_TRIG_FRAC;
62  m_a2c->m[1] = (m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7]) >> INT32_TRIG_FRAC;
63  m_a2c->m[2] = (m_b2c->m[0] * m_a2b->m[2] + m_b2c->m[1] * m_a2b->m[5] + m_b2c->m[2] * m_a2b->m[8]) >> INT32_TRIG_FRAC;
64  m_a2c->m[3] = (m_b2c->m[3] * m_a2b->m[0] + m_b2c->m[4] * m_a2b->m[3] + m_b2c->m[5] * m_a2b->m[6]) >> INT32_TRIG_FRAC;
65  m_a2c->m[4] = (m_b2c->m[3] * m_a2b->m[1] + m_b2c->m[4] * m_a2b->m[4] + m_b2c->m[5] * m_a2b->m[7]) >> INT32_TRIG_FRAC;
66  m_a2c->m[5] = (m_b2c->m[3] * m_a2b->m[2] + m_b2c->m[4] * m_a2b->m[5] + m_b2c->m[5] * m_a2b->m[8]) >> INT32_TRIG_FRAC;
67  m_a2c->m[6] = (m_b2c->m[6] * m_a2b->m[0] + m_b2c->m[7] * m_a2b->m[3] + m_b2c->m[8] * m_a2b->m[6]) >> INT32_TRIG_FRAC;
68  m_a2c->m[7] = (m_b2c->m[6] * m_a2b->m[1] + m_b2c->m[7] * m_a2b->m[4] + m_b2c->m[8] * m_a2b->m[7]) >> INT32_TRIG_FRAC;
69  m_a2c->m[8] = (m_b2c->m[6] * m_a2b->m[2] + m_b2c->m[7] * m_a2b->m[5] + m_b2c->m[8] * m_a2b->m[8]) >> INT32_TRIG_FRAC;
70 }
71 
75 void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
76 {
77  m_a2b->m[0] = (m_b2c->m[0] * m_a2c->m[0] + m_b2c->m[3] * m_a2c->m[3] + m_b2c->m[6] * m_a2c->m[6]) >> INT32_TRIG_FRAC;
78  m_a2b->m[1] = (m_b2c->m[0] * m_a2c->m[1] + m_b2c->m[3] * m_a2c->m[4] + m_b2c->m[6] * m_a2c->m[7]) >> INT32_TRIG_FRAC;
79  m_a2b->m[2] = (m_b2c->m[0] * m_a2c->m[2] + m_b2c->m[3] * m_a2c->m[5] + m_b2c->m[6] * m_a2c->m[8]) >> INT32_TRIG_FRAC;
80  m_a2b->m[3] = (m_b2c->m[1] * m_a2c->m[0] + m_b2c->m[4] * m_a2c->m[3] + m_b2c->m[7] * m_a2c->m[6]) >> INT32_TRIG_FRAC;
81  m_a2b->m[4] = (m_b2c->m[1] * m_a2c->m[1] + m_b2c->m[4] * m_a2c->m[4] + m_b2c->m[7] * m_a2c->m[7]) >> INT32_TRIG_FRAC;
82  m_a2b->m[5] = (m_b2c->m[1] * m_a2c->m[2] + m_b2c->m[4] * m_a2c->m[5] + m_b2c->m[7] * m_a2c->m[8]) >> INT32_TRIG_FRAC;
83  m_a2b->m[6] = (m_b2c->m[2] * m_a2c->m[0] + m_b2c->m[5] * m_a2c->m[3] + m_b2c->m[8] * m_a2c->m[6]) >> INT32_TRIG_FRAC;
84  m_a2b->m[7] = (m_b2c->m[2] * m_a2c->m[1] + m_b2c->m[5] * m_a2c->m[4] + m_b2c->m[8] * m_a2c->m[7]) >> INT32_TRIG_FRAC;
85  m_a2b->m[8] = (m_b2c->m[2] * m_a2c->m[2] + m_b2c->m[5] * m_a2c->m[5] + m_b2c->m[8] * m_a2c->m[8]) >> INT32_TRIG_FRAC;
86 }
87 
91 void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
92 {
93  vb->x = (m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z) >> INT32_TRIG_FRAC;
94  vb->y = (m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z) >> INT32_TRIG_FRAC;
95  vb->z = (m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z) >> INT32_TRIG_FRAC;
96 }
97 
101 void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
102 {
103  vb->x = (m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z) >> INT32_TRIG_FRAC;
104  vb->y = (m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z) >> INT32_TRIG_FRAC;
105  vb->z = (m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z) >> INT32_TRIG_FRAC;
106 }
107 
111 void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b, struct Int32Rates *ra)
112 {
113  rb->p = (m_a2b->m[0] * ra->p + m_a2b->m[1] * ra->q + m_a2b->m[2] * ra->r) >> INT32_TRIG_FRAC;
114  rb->q = (m_a2b->m[3] * ra->p + m_a2b->m[4] * ra->q + m_a2b->m[5] * ra->r) >> INT32_TRIG_FRAC;
115  rb->r = (m_a2b->m[6] * ra->p + m_a2b->m[7] * ra->q + m_a2b->m[8] * ra->r) >> INT32_TRIG_FRAC;
116 }
117 
121 void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
122 {
123  rb->p = (m_b2a->m[0] * ra->p + m_b2a->m[3] * ra->q + m_b2a->m[6] * ra->r) >> INT32_TRIG_FRAC;
124  rb->q = (m_b2a->m[1] * ra->p + m_b2a->m[4] * ra->q + m_b2a->m[7] * ra->r) >> INT32_TRIG_FRAC;
125  rb->r = (m_b2a->m[2] * ra->p + m_b2a->m[5] * ra->q + m_b2a->m[8] * ra->r) >> INT32_TRIG_FRAC;
126 }
127 
128 
132 void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q)
133 {
134  const int32_t _2qi2_m1 = INT_MULT_RSHIFT(q->qi, q->qi,
135  INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1) - TRIG_BFP_OF_REAL(1);
136  rm->m[0] = INT_MULT_RSHIFT(q->qx, q->qx, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
137  rm->m[4] = INT_MULT_RSHIFT(q->qy, q->qy, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
138  rm->m[8] = INT_MULT_RSHIFT(q->qz, q->qz, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
139 
140  const int32_t _2qiqx = INT_MULT_RSHIFT(q->qi, q->qx, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
141  const int32_t _2qiqy = INT_MULT_RSHIFT(q->qi, q->qy, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
142  const int32_t _2qiqz = INT_MULT_RSHIFT(q->qi, q->qz, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
143  rm->m[1] = INT_MULT_RSHIFT(q->qx, q->qy, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
144  rm->m[2] = INT_MULT_RSHIFT(q->qx, q->qz, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
145  rm->m[5] = INT_MULT_RSHIFT(q->qy, q->qz, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1);
146  rm->m[0] += _2qi2_m1;
147  rm->m[3] = rm->m[1] - _2qiqz;
148  rm->m[6] = rm->m[2] + _2qiqy;
149  rm->m[7] = rm->m[5] - _2qiqx;
150  rm->m[4] += _2qi2_m1;
151  rm->m[1] += _2qiqz;
152  rm->m[2] -= _2qiqy;
153  rm->m[5] += _2qiqx;
154  rm->m[8] += _2qi2_m1;
155 }
156 
157 
161 void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e)
162 {
163  int32_t sphi;
164  PPRZ_ITRIG_SIN(sphi, e->phi);
165  int32_t cphi;
166  PPRZ_ITRIG_COS(cphi, e->phi);
167  int32_t stheta;
168  PPRZ_ITRIG_SIN(stheta, e->theta);
169  int32_t ctheta;
170  PPRZ_ITRIG_COS(ctheta, e->theta);
171  int32_t spsi;
172  PPRZ_ITRIG_SIN(spsi, e->psi);
173  int32_t cpsi;
174  PPRZ_ITRIG_COS(cpsi, e->psi);
175 
176  int32_t ctheta_cpsi = INT_MULT_RSHIFT(ctheta, cpsi, INT32_TRIG_FRAC);
177  int32_t ctheta_spsi = INT_MULT_RSHIFT(ctheta, spsi, INT32_TRIG_FRAC);
178  int32_t cphi_spsi = INT_MULT_RSHIFT(cphi, spsi, INT32_TRIG_FRAC);
179  int32_t cphi_cpsi = INT_MULT_RSHIFT(cphi, cpsi, INT32_TRIG_FRAC);
180  int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC);
181  int32_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC);
182  int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC);
183  int32_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC);
184  int32_t sphi_spsi = INT_MULT_RSHIFT(sphi, spsi, INT32_TRIG_FRAC);
185  int32_t sphi_cpsi = INT_MULT_RSHIFT(sphi, cpsi, INT32_TRIG_FRAC);
186 
187  int32_t sphi_stheta_cpsi = INT_MULT_RSHIFT(sphi_stheta, cpsi, INT32_TRIG_FRAC);
188  int32_t sphi_stheta_spsi = INT_MULT_RSHIFT(sphi_stheta, spsi, INT32_TRIG_FRAC);
189  int32_t cphi_stheta_cpsi = INT_MULT_RSHIFT(cphi_stheta, cpsi, INT32_TRIG_FRAC);
190  int32_t cphi_stheta_spsi = INT_MULT_RSHIFT(cphi_stheta, spsi, INT32_TRIG_FRAC);
191 
192  RMAT_ELMT(*rm, 0, 0) = ctheta_cpsi;
193  RMAT_ELMT(*rm, 0, 1) = ctheta_spsi;
194  RMAT_ELMT(*rm, 0, 2) = -stheta;
195  RMAT_ELMT(*rm, 1, 0) = sphi_stheta_cpsi - cphi_spsi;
196  RMAT_ELMT(*rm, 1, 1) = sphi_stheta_spsi + cphi_cpsi;
197  RMAT_ELMT(*rm, 1, 2) = sphi_ctheta;
198  RMAT_ELMT(*rm, 2, 0) = cphi_stheta_cpsi + sphi_spsi;
199  RMAT_ELMT(*rm, 2, 1) = cphi_stheta_spsi - sphi_cpsi;
200  RMAT_ELMT(*rm, 2, 2) = cphi_ctheta;
201 }
202 
203 
204 void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e)
205 {
206  int32_t sphi;
207  PPRZ_ITRIG_SIN(sphi, e->phi);
208  int32_t cphi;
209  PPRZ_ITRIG_COS(cphi, e->phi);
210  int32_t stheta;
211  PPRZ_ITRIG_SIN(stheta, e->theta);
212  int32_t ctheta;
213  PPRZ_ITRIG_COS(ctheta, e->theta);
214  int32_t spsi;
215  PPRZ_ITRIG_SIN(spsi, e->psi);
216  int32_t cpsi;
217  PPRZ_ITRIG_COS(cpsi, e->psi);
218 
219  int32_t stheta_spsi = INT_MULT_RSHIFT(stheta, spsi, INT32_TRIG_FRAC);
220  int32_t stheta_cpsi = INT_MULT_RSHIFT(stheta, cpsi, INT32_TRIG_FRAC);
221  int32_t ctheta_spsi = INT_MULT_RSHIFT(ctheta, spsi, INT32_TRIG_FRAC);
222  int32_t ctheta_cpsi = INT_MULT_RSHIFT(ctheta, cpsi, INT32_TRIG_FRAC);
223  int32_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC);
224  int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC);
225  int32_t cphi_spsi = INT_MULT_RSHIFT(cphi, spsi, INT32_TRIG_FRAC);
226  int32_t cphi_cpsi = INT_MULT_RSHIFT(cphi, cpsi, INT32_TRIG_FRAC);
227  int32_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC);
228  int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC);
229 
230  int32_t sphi_stheta_spsi = INT_MULT_RSHIFT(sphi_stheta, spsi, INT32_TRIG_FRAC);
231  int32_t sphi_stheta_cpsi = INT_MULT_RSHIFT(sphi_stheta, cpsi, INT32_TRIG_FRAC);
232  int32_t sphi_ctheta_spsi = INT_MULT_RSHIFT(sphi_ctheta, spsi, INT32_TRIG_FRAC);
233  int32_t sphi_ctheta_cpsi = INT_MULT_RSHIFT(sphi_ctheta, cpsi, INT32_TRIG_FRAC);
234 
235  RMAT_ELMT(*rm, 0, 0) = ctheta_cpsi - sphi_stheta_spsi;
236  RMAT_ELMT(*rm, 0, 1) = ctheta_spsi + sphi_stheta_cpsi;
237  RMAT_ELMT(*rm, 0, 2) = -cphi_stheta;
238  RMAT_ELMT(*rm, 1, 0) = -cphi_spsi;
239  RMAT_ELMT(*rm, 1, 1) = cphi_cpsi;
240  RMAT_ELMT(*rm, 1, 2) = sphi;
241  RMAT_ELMT(*rm, 2, 0) = stheta_cpsi + sphi_ctheta_spsi;
242  RMAT_ELMT(*rm, 2, 1) = stheta_spsi - sphi_ctheta_cpsi;
243  RMAT_ELMT(*rm, 2, 2) = cphi_ctheta;
244 }
245 
246 
247 /*
248  *
249  * Quaternions
250  *
251  */
252 
253 void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
254 {
255  a2c->qi = (a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz) >> INT32_QUAT_FRAC;
256  a2c->qx = (a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy) >> INT32_QUAT_FRAC;
257  a2c->qy = (a2b->qi * b2c->qy - a2b->qx * b2c->qz + a2b->qy * b2c->qi + a2b->qz * b2c->qx) >> INT32_QUAT_FRAC;
258  a2c->qz = (a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi) >> INT32_QUAT_FRAC;
259 }
260 
261 void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
262 {
263  a2b->qi = (a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz) >> INT32_QUAT_FRAC;
264  a2b->qx = (-a2c->qi * b2c->qx + a2c->qx * b2c->qi - a2c->qy * b2c->qz + a2c->qz * b2c->qy) >> INT32_QUAT_FRAC;
265  a2b->qy = (-a2c->qi * b2c->qy + a2c->qx * b2c->qz + a2c->qy * b2c->qi - a2c->qz * b2c->qx) >> INT32_QUAT_FRAC;
266  a2b->qz = (-a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi) >> INT32_QUAT_FRAC;
267 }
268 
269 void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
270 {
271  b2c->qi = (a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz) >> INT32_QUAT_FRAC;
272  b2c->qx = (a2b->qi * a2c->qx - a2b->qx * a2c->qi - a2b->qy * a2c->qz + a2b->qz * a2c->qy) >> INT32_QUAT_FRAC;
273  b2c->qy = (a2b->qi * a2c->qy + a2b->qx * a2c->qz - a2b->qy * a2c->qi - a2b->qz * a2c->qx) >> INT32_QUAT_FRAC;
274  b2c->qz = (a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi) >> INT32_QUAT_FRAC;
275 }
276 
277 void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
278 {
279  int32_quat_comp(a2c, a2b, b2c);
280  int32_quat_wrap_shortest(a2c);
281  int32_quat_normalize(a2c);
282 }
283 
284 void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
285 {
286  int32_quat_comp_inv(a2b, a2c, b2c);
287  int32_quat_wrap_shortest(a2b);
288  int32_quat_normalize(a2b);
289 }
290 
291 void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
292 {
293  int32_quat_inv_comp(b2c, a2b, a2c);
294  int32_quat_wrap_shortest(b2c);
295  int32_quat_normalize(b2c);
296 }
297 
304 void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q)
305 {
306  qd->qi = (-(r->p * q->qx + r->q * q->qy + r->r * q->qz)) >> (INT32_RATE_FRAC + 1);
307  qd->qx = (-(-r->p * q->qi - r->r * q->qy + r->q * q->qz)) >> (INT32_RATE_FRAC + 1);
308  qd->qy = (-(-r->q * q->qi + r->r * q->qx - r->p * q->qz)) >> (INT32_RATE_FRAC + 1);
309  qd->qz = (-(-r->r * q->qi - r->q * q->qx + r->p * q->qy)) >> (INT32_RATE_FRAC + 1);
310 }
311 
313 void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq)
314 {
315  hr->qi += - ((int64_t) omega->p) * q->qx - ((int64_t) omega->q) * q->qy - ((int64_t) omega->r) * q->qz;
316  hr->qx += ((int64_t) omega->p) * q->qi + ((int64_t) omega->r) * q->qy - ((int64_t) omega->q) * q->qz;
317  hr->qy += ((int64_t) omega->q) * q->qi - ((int64_t) omega->r) * q->qx + ((int64_t) omega->p) * q->qz;
318  hr->qz += ((int64_t) omega->r) * q->qi + ((int64_t) omega->q) * q->qx - ((int64_t) omega->p) * q->qy;
319 
320  lldiv_t _div = lldiv(hr->qi, ((1 << INT32_RATE_FRAC) * freq * 2));
321  q->qi += (int32_t) _div.quot;
322  hr->qi = _div.rem;
323 
324  _div = lldiv(hr->qx, ((1 << INT32_RATE_FRAC) * freq * 2));
325  q->qx += (int32_t) _div.quot;
326  hr->qx = _div.rem;
327 
328  _div = lldiv(hr->qy, ((1 << INT32_RATE_FRAC) * freq * 2));
329  q->qy += (int32_t) _div.quot;
330  hr->qy = _div.rem;
331 
332  _div = lldiv(hr->qz, ((1 << INT32_RATE_FRAC) * freq * 2));
333  q->qz += (int32_t) _div.quot;
334  hr->qz = _div.rem;
335 }
336 
337 void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
338 {
339  const int32_t _2qi2_m1 = ((q->qi * q->qi) >> (INT32_QUAT_FRAC - 1)) - QUAT1_BFP_OF_REAL(1);
340  const int32_t _2qx2 = (q->qx * q->qx) >> (INT32_QUAT_FRAC - 1);
341  const int32_t _2qy2 = (q->qy * q->qy) >> (INT32_QUAT_FRAC - 1);
342  const int32_t _2qz2 = (q->qz * q->qz) >> (INT32_QUAT_FRAC - 1);
343  const int32_t _2qiqx = (q->qi * q->qx) >> (INT32_QUAT_FRAC - 1);
344  const int32_t _2qiqy = (q->qi * q->qy) >> (INT32_QUAT_FRAC - 1);
345  const int32_t _2qiqz = (q->qi * q->qz) >> (INT32_QUAT_FRAC - 1);
346  const int32_t m01 = ((q->qx * q->qy) >> (INT32_QUAT_FRAC - 1)) + _2qiqz;
347  const int32_t m02 = ((q->qx * q->qz) >> (INT32_QUAT_FRAC - 1)) - _2qiqy;
348  const int32_t m12 = ((q->qy * q->qz) >> (INT32_QUAT_FRAC - 1)) + _2qiqx;
349  v_out->x = (_2qi2_m1 * v_in->x + _2qx2 * v_in->x + m01 * v_in->y + m02 * v_in->z) >> INT32_QUAT_FRAC;
350  v_out->y = (_2qi2_m1 * v_in->y + m01 * v_in->x - 2 * _2qiqz * v_in->x + _2qy2 * v_in->y + m12 * v_in->z) >>
351  INT32_QUAT_FRAC;
352  v_out->z = (_2qi2_m1 * v_in->z + m02 * v_in->x + 2 * _2qiqy * v_in->x + m12 * v_in->y - 2 * _2qiqx * v_in->y + _2qz2 *
353  v_in->z) >> INT32_QUAT_FRAC;
354 }
355 
356 /*
357  * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestoquaternions.html
358  */
359 void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
360 {
361  const int32_t phi2 = e->phi / 2;
362  const int32_t theta2 = e->theta / 2;
363  const int32_t psi2 = e->psi / 2;
364 
365  int32_t s_phi2;
366  PPRZ_ITRIG_SIN(s_phi2, phi2);
367  int32_t c_phi2;
368  PPRZ_ITRIG_COS(c_phi2, phi2);
369  int32_t s_theta2;
370  PPRZ_ITRIG_SIN(s_theta2, theta2);
371  int32_t c_theta2;
372  PPRZ_ITRIG_COS(c_theta2, theta2);
373  int32_t s_psi2;
374  PPRZ_ITRIG_SIN(s_psi2, psi2);
375  int32_t c_psi2;
376  PPRZ_ITRIG_COS(c_psi2, psi2);
377 
378  int32_t c_th_c_ps = INT_MULT_RSHIFT(c_theta2, c_psi2, INT32_TRIG_FRAC);
379  int32_t c_th_s_ps = INT_MULT_RSHIFT(c_theta2, s_psi2, INT32_TRIG_FRAC);
380  int32_t s_th_s_ps = INT_MULT_RSHIFT(s_theta2, s_psi2, INT32_TRIG_FRAC);
381  int32_t s_th_c_ps = INT_MULT_RSHIFT(s_theta2, c_psi2, INT32_TRIG_FRAC);
382 
383  q->qi = INT_MULT_RSHIFT(c_phi2, c_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) +
384  INT_MULT_RSHIFT(s_phi2, s_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC);
385  q->qx = INT_MULT_RSHIFT(-c_phi2, s_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) +
386  INT_MULT_RSHIFT(s_phi2, c_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC);
387  q->qy = INT_MULT_RSHIFT(c_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) +
388  INT_MULT_RSHIFT(s_phi2, c_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC);
389  q->qz = INT_MULT_RSHIFT(c_phi2, c_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) +
390  INT_MULT_RSHIFT(-s_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC);
391 }
392 
393 void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle)
394 {
395  int32_t san2;
396  PPRZ_ITRIG_SIN(san2, (angle / 2));
397  int32_t can2;
398  PPRZ_ITRIG_COS(can2, (angle / 2));
399  q->qi = can2;
400  q->qx = san2 * uv->x;
401  q->qy = san2 * uv->y;
402  q->qz = san2 * uv->z;
403 }
404 
405 void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r)
406 {
407  const int32_t tr = RMAT_TRACE(*r);
408  if (tr > 0) {
409  const int32_t two_qi_two = TRIG_BFP_OF_REAL(1.) + tr;
410  uint32_t two_qi = int32_sqrt(two_qi_two << INT32_TRIG_FRAC);
411  two_qi = two_qi << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
412  q->qi = two_qi / 2;
413  q->qx = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) <<
414  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
415  / two_qi;
416  q->qy = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) <<
417  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
418  / two_qi;
419  q->qz = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) <<
420  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
421  / two_qi;
422  } else {
423  if (RMAT_ELMT(*r, 0, 0) > RMAT_ELMT(*r, 1, 1) &&
424  RMAT_ELMT(*r, 0, 0) > RMAT_ELMT(*r, 2, 2)) {
425  const int32_t two_qx_two = RMAT_ELMT(*r, 0, 0) - RMAT_ELMT(*r, 1, 1)
426  - RMAT_ELMT(*r, 2, 2) + TRIG_BFP_OF_REAL(1.);
427  uint32_t two_qx = int32_sqrt(two_qx_two << INT32_TRIG_FRAC);
428  two_qx = two_qx << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
429  q->qi = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) <<
430  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
431  / two_qx;
432  q->qx = two_qx / 2;
433  q->qy = ((RMAT_ELMT(*r, 0, 1) + RMAT_ELMT(*r, 1, 0)) <<
434  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
435  / two_qx;
436  q->qz = ((RMAT_ELMT(*r, 2, 0) + RMAT_ELMT(*r, 0, 2)) <<
437  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
438  / two_qx;
439  } else if (RMAT_ELMT(*r, 1, 1) > RMAT_ELMT(*r, 2, 2)) {
440  const int32_t two_qy_two = RMAT_ELMT(*r, 1, 1) - RMAT_ELMT(*r, 0, 0)
441  - RMAT_ELMT(*r, 2, 2) + TRIG_BFP_OF_REAL(1.);
442  uint32_t two_qy = int32_sqrt(two_qy_two << INT32_TRIG_FRAC);
443  two_qy = two_qy << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
444  q->qi = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) <<
445  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
446  / two_qy;
447  q->qx = ((RMAT_ELMT(*r, 0, 1) + RMAT_ELMT(*r, 1, 0)) <<
448  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
449  / two_qy;
450  q->qy = two_qy / 2;
451  q->qz = ((RMAT_ELMT(*r, 1, 2) + RMAT_ELMT(*r, 2, 1)) <<
452  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
453  / two_qy;
454  } else {
455  const int32_t two_qz_two = RMAT_ELMT(*r, 2, 2) - RMAT_ELMT(*r, 0, 0)
456  - RMAT_ELMT(*r, 1, 1) + TRIG_BFP_OF_REAL(1.);
457  uint32_t two_qz = int32_sqrt(two_qz_two << INT32_TRIG_FRAC);
458  two_qz = two_qz << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
459  q->qi = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) <<
460  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
461  / two_qz;
462  q->qx = ((RMAT_ELMT(*r, 2, 0) + RMAT_ELMT(*r, 0, 2)) <<
463  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
464  / two_qz;
465  q->qy = ((RMAT_ELMT(*r, 1, 2) + RMAT_ELMT(*r, 2, 1)) <<
466  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
467  / two_qz;
468  q->qz = two_qz / 2;
469  }
470  }
471 }
472 
473 
474 /*
475  *
476  * Euler angles
477  *
478  */
479 
480 void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm)
481 {
482  const float dcm00 = TRIG_FLOAT_OF_BFP(rm->m[0]);
483  const float dcm01 = TRIG_FLOAT_OF_BFP(rm->m[1]);
484  const float dcm02 = TRIG_FLOAT_OF_BFP(rm->m[2]);
485  const float dcm12 = TRIG_FLOAT_OF_BFP(rm->m[5]);
486  const float dcm22 = TRIG_FLOAT_OF_BFP(rm->m[8]);
487  const float phi = atan2f(dcm12, dcm22);
488  const float theta = -asinf(dcm02);
489  const float psi = atan2f(dcm01, dcm00);
490  e->phi = ANGLE_BFP_OF_REAL(phi);
491  e->theta = ANGLE_BFP_OF_REAL(theta);
492  e->psi = ANGLE_BFP_OF_REAL(psi);
493 }
494 
495 void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
496 {
497  const int32_t qx2 = INT_MULT_RSHIFT(q->qx, q->qx, INT32_QUAT_FRAC);
498  const int32_t qy2 = INT_MULT_RSHIFT(q->qy, q->qy, INT32_QUAT_FRAC);
499  const int32_t qz2 = INT_MULT_RSHIFT(q->qz, q->qz, INT32_QUAT_FRAC);
500  const int32_t qiqx = INT_MULT_RSHIFT(q->qi, q->qx, INT32_QUAT_FRAC);
501  const int32_t qiqy = INT_MULT_RSHIFT(q->qi, q->qy, INT32_QUAT_FRAC);
502  const int32_t qiqz = INT_MULT_RSHIFT(q->qi, q->qz, INT32_QUAT_FRAC);
503  const int32_t qxqy = INT_MULT_RSHIFT(q->qx, q->qy, INT32_QUAT_FRAC);
504  const int32_t qxqz = INT_MULT_RSHIFT(q->qx, q->qz, INT32_QUAT_FRAC);
505  const int32_t qyqz = INT_MULT_RSHIFT(q->qy, q->qz, INT32_QUAT_FRAC);
506  const int32_t one = TRIG_BFP_OF_REAL(1);
507  const int32_t two = TRIG_BFP_OF_REAL(2);
508 
509  /* dcm00 = 1.0 - 2.*( qy2 + qz2 ); */
510  const int32_t idcm00 = one - INT_MULT_RSHIFT(two, (qy2 + qz2),
511  INT32_TRIG_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC);
512  /* dcm01 = 2.*( qxqy + qiqz ); */
513  const int32_t idcm01 = INT_MULT_RSHIFT(two, (qxqy + qiqz),
514  INT32_TRIG_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC);
515  /* dcm02 = 2.*( qxqz - qiqy ); */
516  const int32_t idcm02 = INT_MULT_RSHIFT(two, (qxqz - qiqy),
517  INT32_TRIG_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC);
518  /* dcm12 = 2.*( qyqz + qiqx ); */
519  const int32_t idcm12 = INT_MULT_RSHIFT(two, (qyqz + qiqx),
520  INT32_TRIG_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC);
521  /* dcm22 = 1.0 - 2.*( qx2 + qy2 ); */
522  const int32_t idcm22 = one - INT_MULT_RSHIFT(two, (qx2 + qy2),
523  INT32_TRIG_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC);
524  const float dcm00 = (float)idcm00 / (1 << INT32_TRIG_FRAC);
525  const float dcm01 = (float)idcm01 / (1 << INT32_TRIG_FRAC);
526  const float dcm02 = (float)idcm02 / (1 << INT32_TRIG_FRAC);
527  const float dcm12 = (float)idcm12 / (1 << INT32_TRIG_FRAC);
528  const float dcm22 = (float)idcm22 / (1 << INT32_TRIG_FRAC);
529 
530  const float phi = atan2f(dcm12, dcm22);
531  const float theta = -asinf(dcm02);
532  const float psi = atan2f(dcm01, dcm00);
533  e->phi = ANGLE_BFP_OF_REAL(phi);
534  e->theta = ANGLE_BFP_OF_REAL(theta);
535  e->psi = ANGLE_BFP_OF_REAL(psi);
536 }
537 
538 
539 /*
540  *
541  * Rotational speeds
542  *
543  */
544 
545 void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed)
546 {
547  int32_t sphi;
548  PPRZ_ITRIG_SIN(sphi, e->phi);
549  int32_t cphi;
550  PPRZ_ITRIG_COS(cphi, e->phi);
551  int32_t stheta;
552  PPRZ_ITRIG_SIN(stheta, e->theta);
553  int32_t ctheta;
554  PPRZ_ITRIG_COS(ctheta, e->theta);
555 
556  int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC);
557  int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC);
558 
559  r->p = - INT_MULT_RSHIFT(stheta, ed->psi, INT32_TRIG_FRAC) + ed->phi;
560  r->q = INT_MULT_RSHIFT(sphi_ctheta, ed->psi, INT32_TRIG_FRAC) + INT_MULT_RSHIFT(cphi, ed->theta, INT32_TRIG_FRAC);
561  r->r = INT_MULT_RSHIFT(cphi_ctheta, ed->psi, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, ed->theta, INT32_TRIG_FRAC);
562 }
563 
564 void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r)
565 {
566  int32_t sphi;
567  PPRZ_ITRIG_SIN(sphi, e->phi);
568  int32_t cphi;
569  PPRZ_ITRIG_COS(cphi, e->phi);
570  int32_t stheta;
571  PPRZ_ITRIG_SIN(stheta, e->theta);
572  int64_t ctheta;
573  PPRZ_ITRIG_COS(ctheta, e->theta);
574 
575  if (ctheta != 0) {
576  int64_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC);
577  int64_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC);
578 
579  ed->phi = r->p + (int32_t)((sphi_stheta * (int64_t)r->q) / ctheta) + (int32_t)((cphi_stheta * (int64_t)r->r) / ctheta);
580  ed->theta = INT_MULT_RSHIFT(cphi, r->q, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, r->r, INT32_TRIG_FRAC);
581  ed->psi = (int32_t)(((int64_t)sphi * (int64_t)r->q) / ctheta) + (int32_t)(((int64_t)cphi * (int64_t)r->r) / ctheta);
582  }
583  /* FIXME: What do you wanna do when you hit the singularity ? */
584  /* probably not return an uninitialized variable, or ? */
585  else {
586  INT_EULERS_ZERO(*ed);
587  }
588 }
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
INT32_RATE_FRAC
#define INT32_RATE_FRAC
Definition: pprz_algebra_int.h:117
Int32Quat::qz
int32_t qz
Definition: pprz_algebra_int.h:103
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
int32_quat_normalize
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
Definition: pprz_algebra_int.h:454
int32_quat_of_rmat
void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r)
Quaternion from rotation matrix.
Definition: pprz_algebra_int.c:405
int32_quat_inv_comp
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:269
int32_quat_comp
void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:253
int32_quat_of_axis_angle
void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle)
Quaternion from unit vector and angle.
Definition: pprz_algebra_int.c:393
Int32Quat::qi
int32_t qi
Definition: pprz_algebra_int.h:100
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
int32_quat_of_eulers
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
Definition: pprz_algebra_int.c:359
TRIG_FLOAT_OF_BFP
#define TRIG_FLOAT_OF_BFP(_ti)
Definition: pprz_algebra_int.h:215
Int32RMat::m
int32_t m[3 *3]
Definition: pprz_algebra_int.h:160
QUAT1_BFP_OF_REAL
#define QUAT1_BFP_OF_REAL(_qf)
Definition: pprz_algebra_int.h:212
int32_quat_integrate_fi
void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq)
in place quaternion first order integration with constant rotational velocity.
Definition: pprz_algebra_int.c:313
int32_rmat_of_eulers_312
void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e)
Rotation matrix from 312 Euler angles.
Definition: pprz_algebra_int.c:204
int32_quat_derivative
void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q)
Quaternion derivative from rotational velocity.
Definition: pprz_algebra_int.c:304
Int64Quat::qz
int64_t qz
Definition: pprz_algebra_int.h:111
int64_t
signed long long int64_t
Definition: types.h:21
int32_quat_comp_inv_norm_shortest
void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_int.c:284
int32_rmat_ratemult
void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b, struct Int32Rates *ra)
rotate anglular rates by rotation matrix.
Definition: pprz_algebra_int.c:111
Int32Quat::qx
int32_t qx
Definition: pprz_algebra_int.h:101
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:109
int32_rmat_comp_inv
void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_int.c:75
int32_rmat_vmult
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_int.c:91
Int32Vect3
Definition: pprz_algebra_int.h:88
TRIG_BFP_OF_REAL
#define TRIG_BFP_OF_REAL(_tf)
Definition: pprz_algebra_int.h:214
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
RMAT_TRACE
#define RMAT_TRACE(_rm)
Definition: pprz_algebra.h:596
int32_quat_inv_comp_norm_shortest
void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_int.c:291
int32_quat_comp_norm_shortest
void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_int.c:277
int32_rates_of_eulers_dot_321
void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed)
Definition: pprz_algebra_int.c:545
int32_rmat_comp
void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_int.c:59
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
uint32_t
unsigned long uint32_t
Definition: types.h:18
int32_sqrt
uint32_t int32_sqrt(uint32_t in)
Definition: pprz_algebra_int.c:30
int32_quat_comp_inv
void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:261
INT32_QUAT_FRAC
#define INT32_QUAT_FRAC
Definition: pprz_algebra_int.h:95
int32_quat_vmult
void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
rotate 3D vector by quaternion.
Definition: pprz_algebra_int.c:337
int32_t
signed long int32_t
Definition: types.h:19
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
int32_eulers_dot_321_of_rates
void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r)
Definition: pprz_algebra_int.c:564
int32_eulers_of_quat
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
Definition: pprz_algebra_int.c:495
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
INT_EULERS_ZERO
#define INT_EULERS_ZERO(_e)
Definition: pprz_algebra_int.h:544
uint8_t
unsigned char uint8_t
Definition: types.h:14
INT_MULT_RSHIFT
#define INT_MULT_RSHIFT(_a, _b, _r)
Definition: pprz_algebra_int.h:225
RMAT_ELMT
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:593
INT32_SQRT_MAX_ITER
#define INT32_SQRT_MAX_ITER
Definition: pprz_algebra_int.c:29
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
int32_quat_wrap_shortest
static void int32_quat_wrap_shortest(struct Int32Quat *q)
Definition: pprz_algebra_int.h:446
int32_eulers_of_rmat
void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm)
Definition: pprz_algebra_int.c:480
int32_rmat_transp_vmult
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_int.c:101
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
int32_rmat_of_eulers_321
void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e)
Rotation matrix from 321 Euler angles.
Definition: pprz_algebra_int.c:161
Int64Quat::qy
int64_t qy
Definition: pprz_algebra_int.h:110
int32_rmat_transp_ratemult
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
Definition: pprz_algebra_int.c:121
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
Int64Quat::qi
int64_t qi
Definition: pprz_algebra_int.h:108
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
int32_rmat_of_quat
void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q)
Convert unit quaternion to rotation matrix.
Definition: pprz_algebra_int.c:132
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:110
pprz_algebra_int.h
Paparazzi fixed point algebra.
Int64Quat
Definition: pprz_algebra_int.h:107
Int64Quat::qx
int64_t qx
Definition: pprz_algebra_int.h:109
Int32Quat::qy
int32_t qy
Definition: pprz_algebra_int.h:102