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