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_float.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_float.h"
28 
30 void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
31 {
32  vec->x += dv->x * dt;
33  vec->y += dv->y * dt;
34  vec->z += dv->z * dt;
35 }
36 
38 void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt)
39 {
40  r->p += dr->p * dt;
41  r->q += dr->q * dt;
42  r->r += dr->r * dt;
43 }
44 
45 void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
46 {
47  r->p = edot->phi - sinf(e->theta) * edot->psi;
48  r->q = cosf(e->phi) * edot->theta + sinf(e->phi) * cosf(e->theta) * edot->psi;
49  r->r = -sinf(e->phi) * edot->theta + cosf(e->phi) * cosf(e->theta) * edot->psi;
50 }
51 
52 
53 
54 
55 void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
56 {
57  RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);
58  RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
59  RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
60  RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
61  RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);
62  RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
63  RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
64  RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
65  RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);
66 }
67 
68 float float_rmat_norm(struct FloatRMat *rm)
69 {
70  return sqrtf(SQUARE(rm->m[0]) + SQUARE(rm->m[1]) + SQUARE(rm->m[2]) +
71  SQUARE(rm->m[3]) + SQUARE(rm->m[4]) + SQUARE(rm->m[5]) +
72  SQUARE(rm->m[6]) + SQUARE(rm->m[7]) + SQUARE(rm->m[8]));
73 }
74 
78 void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
79 {
80  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];
81  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];
82  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];
83  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];
84  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];
85  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];
86  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];
87  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];
88  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];
89 }
90 
94 void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
95 {
96  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];
97  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];
98  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];
99  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];
100  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];
101  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];
102  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];
103  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];
104  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];
105 }
106 
110 void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
111 {
112  vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
113  vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
114  vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
115 }
116 
120 void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
121 {
122  vb->x = m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z;
123  vb->y = m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z;
124  vb->z = m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z;
125 }
126 
130 void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
131 {
132  rb->p = m_a2b->m[0] * ra->p + m_a2b->m[1] * ra->q + m_a2b->m[2] * ra->r;
133  rb->q = m_a2b->m[3] * ra->p + m_a2b->m[4] * ra->q + m_a2b->m[5] * ra->r;
134  rb->r = m_a2b->m[6] * ra->p + m_a2b->m[7] * ra->q + m_a2b->m[8] * ra->r;
135 }
136 
140 void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
141 {
142  rb->p = m_b2a->m[0] * ra->p + m_b2a->m[3] * ra->q + m_b2a->m[6] * ra->r;
143  rb->q = m_b2a->m[1] * ra->p + m_b2a->m[4] * ra->q + m_b2a->m[7] * ra->r;
144  rb->r = m_b2a->m[2] * ra->p + m_b2a->m[5] * ra->q + m_b2a->m[8] * ra->r;
145 }
146 
147 
149 void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
150 {
151  const float ux2 = uv->x * uv->x;
152  const float uy2 = uv->y * uv->y;
153  const float uz2 = uv->z * uv->z;
154  const float uxuy = uv->x * uv->y;
155  const float uyuz = uv->y * uv->z;
156  const float uxuz = uv->x * uv->z;
157  const float can = cosf(angle);
158  const float san = sinf(angle);
159  const float one_m_can = (1. - can);
160 
161  RMAT_ELMT(*rm, 0, 0) = ux2 + (1. - ux2) * can;
162  RMAT_ELMT(*rm, 0, 1) = uxuy * one_m_can + uv->z * san;
163  RMAT_ELMT(*rm, 0, 2) = uxuz * one_m_can - uv->y * san;
164  RMAT_ELMT(*rm, 1, 0) = RMAT_ELMT(*rm, 0, 1);
165  RMAT_ELMT(*rm, 1, 1) = uy2 + (1. - uy2) * can;
166  RMAT_ELMT(*rm, 1, 2) = uyuz * one_m_can + uv->x * san;
167  RMAT_ELMT(*rm, 2, 0) = RMAT_ELMT(*rm, 0, 2);
168  RMAT_ELMT(*rm, 2, 1) = RMAT_ELMT(*rm, 1, 2);
169  RMAT_ELMT(*rm, 2, 2) = uz2 + (1. - uz2) * can;
170 }
171 
172 
173 /* C n->b rotation matrix */
174 void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
175 {
176  const float sphi = sinf(e->phi);
177  const float cphi = cosf(e->phi);
178  const float stheta = sinf(e->theta);
179  const float ctheta = cosf(e->theta);
180  const float spsi = sinf(e->psi);
181  const float cpsi = cosf(e->psi);
182 
183  RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi;
184  RMAT_ELMT(*rm, 0, 1) = ctheta * spsi;
185  RMAT_ELMT(*rm, 0, 2) = -stheta;
186  RMAT_ELMT(*rm, 1, 0) = sphi * stheta * cpsi - cphi * spsi;
187  RMAT_ELMT(*rm, 1, 1) = sphi * stheta * spsi + cphi * cpsi;
188  RMAT_ELMT(*rm, 1, 2) = sphi * ctheta;
189  RMAT_ELMT(*rm, 2, 0) = cphi * stheta * cpsi + sphi * spsi;
190  RMAT_ELMT(*rm, 2, 1) = cphi * stheta * spsi - sphi * cpsi;
191  RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
192 }
193 
194 void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
195 {
196  const float sphi = sinf(e->phi);
197  const float cphi = cosf(e->phi);
198  const float stheta = sinf(e->theta);
199  const float ctheta = cosf(e->theta);
200  const float spsi = sinf(e->psi);
201  const float cpsi = cosf(e->psi);
202 
203  RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi - sphi * stheta * spsi;
204  RMAT_ELMT(*rm, 0, 1) = ctheta * spsi + sphi * stheta * cpsi;
205  RMAT_ELMT(*rm, 0, 2) = -cphi * stheta;
206  RMAT_ELMT(*rm, 1, 0) = -cphi * spsi;
207  RMAT_ELMT(*rm, 1, 1) = cphi * cpsi;
208  RMAT_ELMT(*rm, 1, 2) = sphi;
209  RMAT_ELMT(*rm, 2, 0) = stheta * cpsi + sphi * ctheta * spsi;
210  RMAT_ELMT(*rm, 2, 1) = stheta * spsi - sphi * ctheta * cpsi;
211  RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
212 }
213 
214 
215 /* C n->b rotation matrix */
216 void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
217 {
218  const float _a = M_SQRT2 * q->qi;
219  const float _b = M_SQRT2 * q->qx;
220  const float _c = M_SQRT2 * q->qy;
221  const float _d = M_SQRT2 * q->qz;
222  const float a2_1 = _a * _a - 1;
223  const float ab = _a * _b;
224  const float ac = _a * _c;
225  const float ad = _a * _d;
226  const float bc = _b * _c;
227  const float bd = _b * _d;
228  const float cd = _c * _d;
229  RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
230  RMAT_ELMT(*rm, 0, 1) = bc + ad;
231  RMAT_ELMT(*rm, 0, 2) = bd - ac;
232  RMAT_ELMT(*rm, 1, 0) = bc - ad;
233  RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
234  RMAT_ELMT(*rm, 1, 2) = cd + ab;
235  RMAT_ELMT(*rm, 2, 0) = bd + ac;
236  RMAT_ELMT(*rm, 2, 1) = cd - ab;
237  RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
238 }
239 
241 void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
242 {
243  struct FloatRMat exp_omega_dt = {
244  {
245  1. , dt *omega->r, -dt *omega->q,
246  -dt *omega->r, 1. , dt *omega->p,
247  dt *omega->q, -dt *omega->p, 1.
248  }
249  };
250  struct FloatRMat R_tdt;
251  FLOAT_RMAT_COMP(R_tdt, *rm, exp_omega_dt);
252  memcpy(rm, &R_tdt, sizeof(R_tdt));
253 }
254 
255 static inline float renorm_factor(float n)
256 {
257  if (n < 1.5625f && n > 0.64f) {
258  return .5 * (3 - n);
259  } else if (n < 100.0f && n > 0.01f) {
260  return 1. / sqrtf(n);
261  } else {
262  return 0.;
263  }
264 }
265 
267 {
268  const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0),
269  RMAT_ELMT(*rm, 0, 1),
270  RMAT_ELMT(*rm, 0, 2)
271  };
272  const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1, 0),
273  RMAT_ELMT(*rm, 1, 1),
274  RMAT_ELMT(*rm, 1, 2)
275  };
276  float _err = -0.5 * VECT3_DOT_PRODUCT(r0, r1);
277  struct FloatVect3 r0_t;
278  VECT3_SUM_SCALED(r0_t, r0, r1, _err);
279  struct FloatVect3 r1_t;
280  VECT3_SUM_SCALED(r1_t, r1, r0, _err);
281  struct FloatVect3 r2_t;
282  VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
283  float s = renorm_factor(VECT3_NORM2(r0_t));
284  MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
285  s = renorm_factor(VECT3_NORM2(r1_t));
286  MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
287  s = renorm_factor(VECT3_NORM2(r2_t));
288  MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
289 
290  return _err;
291 }
292 
293 
294 /*
295  *
296  * Quaternion functions.
297  *
298  */
299 
300 void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
301 {
302  a2c->qi = a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz;
303  a2c->qx = a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy;
304  a2c->qy = a2b->qi * b2c->qy - a2b->qx * b2c->qz + a2b->qy * b2c->qi + a2b->qz * b2c->qx;
305  a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi;
306 }
307 
308 void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
309 {
310  a2b->qi = a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz;
311  a2b->qx = -a2c->qi * b2c->qx + a2c->qx * b2c->qi - a2c->qy * b2c->qz + a2c->qz * b2c->qy;
312  a2b->qy = -a2c->qi * b2c->qy + a2c->qx * b2c->qz + a2c->qy * b2c->qi - a2c->qz * b2c->qx;
313  a2b->qz = -a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi;
314 }
315 
316 void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
317 {
318  b2c->qi = a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz;
319  b2c->qx = a2b->qi * a2c->qx - a2b->qx * a2c->qi - a2b->qy * a2c->qz + a2b->qz * a2c->qy;
320  b2c->qy = a2b->qi * a2c->qy + a2b->qx * a2c->qz - a2b->qy * a2c->qi - a2b->qz * a2c->qx;
321  b2c->qz = a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi;
322 }
323 
324 void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
325 {
326  float_quat_comp(a2c, a2b, b2c);
329 }
330 
331 void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
332 {
333  float_quat_comp_inv(a2b, a2c, b2c);
336 }
337 
338 void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
339 {
340  float_quat_inv_comp(b2c, a2b, a2c);
343 }
344 
345 void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
346 {
347  const float v_norm = sqrtf(w->p * w->p + w->q * w->q + w->r * w->r);
348  const float c2 = cos(dt * v_norm / 2.0);
349  const float s2 = sin(dt * v_norm / 2.0);
350  if (v_norm < 1e-8) {
351  q_out->qi = 1;
352  q_out->qx = 0;
353  q_out->qy = 0;
354  q_out->qz = 0;
355  } else {
356  q_out->qi = c2;
357  q_out->qx = w->p / v_norm * s2;
358  q_out->qy = w->q / v_norm * s2;
359  q_out->qz = w->r / v_norm * s2;
360  }
361 }
362 
364 void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
365 {
366  const float qi = q->qi;
367  const float qx = q->qx;
368  const float qy = q->qy;
369  const float qz = q->qz;
370  const float dp = 0.5 * dt * omega->p;
371  const float dq = 0.5 * dt * omega->q;
372  const float dr = 0.5 * dt * omega->r;
373  q->qi = qi - dp * qx - dq * qy - dr * qz;
374  q->qx = dp * qi + qx + dr * qy - dq * qz;
375  q->qy = dq * qi - dr * qx + qy + dp * qz;
376  q->qz = dr * qi + dq * qx - dp * qy + qz;
377 }
378 
380 void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
381 {
382  const float no = FLOAT_RATES_NORM(*omega);
383  if (no > FLT_MIN) {
384  const float a = 0.5 * no * dt;
385  const float ca = cosf(a);
386  const float sa_ov_no = sinf(a) / no;
387  const float dp = sa_ov_no * omega->p;
388  const float dq = sa_ov_no * omega->q;
389  const float dr = sa_ov_no * omega->r;
390  const float qi = q->qi;
391  const float qx = q->qx;
392  const float qy = q->qy;
393  const float qz = q->qz;
394  q->qi = ca * qi - dp * qx - dq * qy - dr * qz;
395  q->qx = dp * qi + ca * qx + dr * qy - dq * qz;
396  q->qy = dq * qi - dr * qx + ca * qy + dp * qz;
397  q->qz = dr * qi + dq * qx - dp * qy + ca * qz;
398  }
399 }
400 
401 void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
402 {
403  const float qi2_M1_2 = q->qi * q->qi - 0.5;
404  const float qiqx = q->qi * q->qx;
405  const float qiqy = q->qi * q->qy;
406  const float qiqz = q->qi * q->qz;
407  float m01 = q->qx * q->qy; /* aka qxqy */
408  float m02 = q->qx * q->qz; /* aka qxqz */
409  float m12 = q->qy * q->qz; /* aka qyqz */
410 
411  const float m00 = qi2_M1_2 + q->qx * q->qx;
412  const float m10 = m01 - qiqz;
413  const float m20 = m02 + qiqy;
414  const float m21 = m12 - qiqx;
415  m01 += qiqz;
416  m02 -= qiqy;
417  m12 += qiqx;
418  const float m11 = qi2_M1_2 + q->qy * q->qy;
419  const float m22 = qi2_M1_2 + q->qz * q->qz;
420  v_out->x = 2 * (m00 * v_in->x + m01 * v_in->y + m02 * v_in->z);
421  v_out->y = 2 * (m10 * v_in->x + m11 * v_in->y + m12 * v_in->z);
422  v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
423 }
424 
430 void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
431 {
432  qd->qi = -0.5 * (r->p * q->qx + r->q * q->qy + r->r * q->qz);
433  qd->qx = -0.5 * (-r->p * q->qi - r->r * q->qy + r->q * q->qz);
434  qd->qy = -0.5 * (-r->q * q->qi + r->r * q->qx - r->p * q->qz);
435  qd->qz = -0.5 * (-r->r * q->qi - r->q * q->qx + r->p * q->qy);
436 }
437 
441 void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
442 {
443  const float K_LAGRANGE = 1.;
444  const float c = K_LAGRANGE * (1 - float_quat_norm(q)) / -0.5;
445  qd->qi = -0.5 * (c * q->qi + r->p * q->qx + r->q * q->qy + r->r * q->qz);
446  qd->qx = -0.5 * (-r->p * q->qi + c * q->qx - r->r * q->qy + r->q * q->qz);
447  qd->qy = -0.5 * (-r->q * q->qi + r->r * q->qx + c * q->qy - r->p * q->qz);
448  qd->qz = -0.5 * (-r->r * q->qi - r->q * q->qx + r->p * q->qy + c * q->qz);
449 }
450 
451 void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
452 {
453 
454  const float phi2 = e->phi / 2.0;
455  const float theta2 = e->theta / 2.0;
456  const float psi2 = e->psi / 2.0;
457 
458  const float s_phi2 = sinf(phi2);
459  const float c_phi2 = cosf(phi2);
460  const float s_theta2 = sinf(theta2);
461  const float c_theta2 = cosf(theta2);
462  const float s_psi2 = sinf(psi2);
463  const float c_psi2 = cosf(psi2);
464 
465  q->qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2;
466  q->qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2;
467  q->qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2;
468  q->qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2;
469 }
470 
471 void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
472 {
473  const float san = sinf(angle / 2.);
474  q->qi = cosf(angle / 2.);
475  q->qx = san * uv->x;
476  q->qy = san * uv->y;
477  q->qz = san * uv->z;
478 }
479 
480 void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
481 {
482  const float ov_norm = sqrtf(ov->x * ov->x + ov->y * ov->y + ov->z * ov->z);
483  if (ov_norm < 1e-8) {
484  q->qi = 1;
485  q->qx = 0;
486  q->qy = 0;
487  q->qz = 0;
488  } else {
489  const float s2_normalized = sinf(ov_norm / 2.0) / ov_norm;
490  q->qi = cosf(ov_norm / 2.0);
491  q->qx = ov->x * s2_normalized;
492  q->qy = ov->y * s2_normalized;
493  q->qz = ov->z * s2_normalized;
494  }
495 }
496 
497 void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
498 {
499  const float tr = RMAT_TRACE(*rm);
500  if (tr > 0) {
501  const float two_qi = sqrtf(1. + tr);
502  const float four_qi = 2. * two_qi;
503  q->qi = 0.5 * two_qi;
504  q->qx = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qi;
505  q->qy = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qi;
506  q->qz = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qi;
507  /*printf("tr > 0\n");*/
508  } else {
509  if (RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 1, 1) &&
510  RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 2, 2)) {
511  const float two_qx = sqrtf(RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1)
512  - RMAT_ELMT(*rm, 2, 2) + 1);
513  const float four_qx = 2. * two_qx;
514  q->qi = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qx;
515  q->qx = 0.5 * two_qx;
516  q->qy = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qx;
517  q->qz = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qx;
518  /*printf("m00 largest\n");*/
519  } else if (RMAT_ELMT(*rm, 1, 1) > RMAT_ELMT(*rm, 2, 2)) {
520  const float two_qy =
521  sqrtf(RMAT_ELMT(*rm, 1, 1) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 2, 2) + 1);
522  const float four_qy = 2. * two_qy;
523  q->qi = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qy;
524  q->qx = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qy;
525  q->qy = 0.5 * two_qy;
526  q->qz = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qy;
527  /*printf("m11 largest\n");*/
528  } else {
529  const float two_qz =
530  sqrtf(RMAT_ELMT(*rm, 2, 2) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1) + 1);
531  const float four_qz = 2. * two_qz;
532  q->qi = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qz;
533  q->qx = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qz;
534  q->qy = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qz;
535  q->qz = 0.5 * two_qz;
536  /*printf("m22 largest\n");*/
537  }
538  }
539 }
540 
541 
542 /*
543  *
544  * Euler angle functions.
545  *
546  */
547 
548 void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
549 {
550  const float dcm00 = rm->m[0];
551  const float dcm01 = rm->m[1];
552  const float dcm02 = rm->m[2];
553  const float dcm12 = rm->m[5];
554  const float dcm22 = rm->m[8];
555  e->phi = atan2f(dcm12, dcm22);
556  e->theta = -asinf(dcm02);
557  e->psi = atan2f(dcm01, dcm00);
558 }
559 
560 void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
561 {
562  const float qx2 = q->qx * q->qx;
563  const float qy2 = q->qy * q->qy;
564  const float qz2 = q->qz * q->qz;
565  const float qiqx = q->qi * q->qx;
566  const float qiqy = q->qi * q->qy;
567  const float qiqz = q->qi * q->qz;
568  const float qxqy = q->qx * q->qy;
569  const float qxqz = q->qx * q->qz;
570  const float qyqz = q->qy * q->qz;
571  const float dcm00 = 1.0 - 2.*(qy2 + qz2);
572  const float dcm01 = 2.*(qxqy + qiqz);
573  const float dcm02 = 2.*(qxqz - qiqy);
574  const float dcm12 = 2.*(qyqz + qiqx);
575  const float dcm22 = 1.0 - 2.*(qx2 + qy2);
576 
577  e->phi = atan2f(dcm12, dcm22);
578  e->theta = -asinf(dcm02);
579  e->psi = atan2f(dcm01, dcm00);
580 }
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:243
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:249
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
#define FLOAT_RATES_NORM(_v)
void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
Quaternion from Euler angles.
float phi
in radians
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
in place first order integration of a 3D-vector
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
float r
in rad/s
float float_rmat_norm(struct FloatRMat *rm)
Norm of a rotation matrix.
float psi
in radians
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:174
void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
#define SQUARE(_a)
Definition: pprz_algebra.h:47
float q
in rad/s
float p
in rad/s
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
euler angles
Roation quaternion.
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
float theta
in radians
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Paparazzi floating point algebra.
#define RMAT_TRACE(_rm)
Definition: pprz_algebra.h:596
void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt)
in place first order integration of angular rates
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
static void float_quat_normalize(struct FloatQuat *q)
void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
Quaternion from rotation matrix.
static float renorm_factor(float n)
#define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s)
Definition: pprz_algebra.h:502
void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
float m[3 *3]
float float_rmat_reorthogonalize(struct FloatRMat *rm)
#define M_SQRT2
#define VECT3_NORM2(_v)
Definition: pprz_algebra.h:251
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
Rotation matrix from 321 Euler angles (float).
void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
initialises a rotation matrix from unit vector axis and angle
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:593
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
Quaternion from unit vector and angle.
static void float_quat_wrap_shortest(struct FloatQuat *q)
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
static uint16_t c2
Definition: baro_MS5534A.c:203
void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
rotation matrix
static float float_quat_norm(struct FloatQuat *q)
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)
void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
Delta rotation quaternion with constant angular rates.
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place first order quaternion integration with constant rotational velocity
angular rates