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