Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ahrs_float_cmpl.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2010-2013 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, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21
29
35#include "generated/airframe.h"
36#if USE_GPS
37#include "modules/gps/gps.h"
38#endif
39
40//#include "../../test/pprz_algebra_print.h"
41
42#if AHRS_PROPAGATE_RMAT && AHRS_PROPAGATE_QUAT
43#error "You can only define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT, not both!"
44#endif
45#if !AHRS_PROPAGATE_RMAT && !AHRS_PROPAGATE_QUAT
46#error "You have to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT"
47#endif
48
49#if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
50#warning "Using magnetometer _and_ GPS course to update heading. Probably better to <configure name="USE_MAGNETOMETER" value="0"/> if you want to use GPS course."
51#endif
52
53/*
54 * default gains for correcting attitude and bias from accel/mag
55 */
56#ifndef AHRS_ACCEL_OMEGA
57#define AHRS_ACCEL_OMEGA 0.063
58#endif
59#ifndef AHRS_ACCEL_ZETA
60#define AHRS_ACCEL_ZETA 0.9
61#endif
62
63#ifndef AHRS_MAG_OMEGA
64#define AHRS_MAG_OMEGA 0.04
65#endif
66#ifndef AHRS_MAG_ZETA
67#define AHRS_MAG_ZETA 0.9
68#endif
69
71#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
72#define AHRS_GRAVITY_HEURISTIC_FACTOR 30
73#endif
74
75
76void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt);
77void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt);
79
81
82void ahrs_fc_init(void)
83{
84 ahrs_fc.status = AHRS_FC_UNINIT;
85 ahrs_fc.is_aligned = false;
86
87 ahrs_fc.ltp_vel_norm_valid = false;
88 ahrs_fc.heading_aligned = false;
89
90 /* init ltp_to_body quaternion as zero/identity rotation */
91 float_quat_identity(&ahrs_fc.ltp_to_body_quat);
92 float_rmat_identity(&ahrs_fc.ltp_to_body_rmat);
93 FLOAT_RATES_ZERO(ahrs_fc.body_rate);
94
95 /* set default filter cut-off frequency and damping */
96 ahrs_fc.accel_omega = AHRS_ACCEL_OMEGA;
97 ahrs_fc.accel_zeta = AHRS_ACCEL_ZETA;
98 ahrs_fc.mag_omega = AHRS_MAG_OMEGA;
99 ahrs_fc.mag_zeta = AHRS_MAG_ZETA;
100
101#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
102 ahrs_fc.correct_gravity = true;
103#else
104 ahrs_fc.correct_gravity = false;
105#endif
106
107 ahrs_fc.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
108
109 VECT3_ASSIGN(ahrs_fc.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
110
111 ahrs_fc.accel_cnt = 0;
112 ahrs_fc.mag_cnt = 0;
113}
114
115bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
116 struct FloatVect3 *lp_mag __attribute__((unused)))
117{
118
119#if USE_MAGNETOMETER
120 /* Compute an initial orientation from accel and mag directly as quaternion */
121 ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_body_quat, lp_accel, lp_mag);
122 ahrs_fc.heading_aligned = true;
123#else
124 /* Compute an initial orientation from accel and just set heading to zero */
125 ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_body_quat, lp_accel);
126 ahrs_fc.heading_aligned = false;
127#endif
128
129 /* Convert initial orientation from quat to rotation matrix representations. */
130 float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
131
132 /* used averaged gyro as initial value for bias */
133 ahrs_fc.gyro_bias = *lp_gyro;
134
135 ahrs_fc.status = AHRS_FC_RUNNING;
136 ahrs_fc.is_aligned = true;
137
138 return true;
139}
140
141
142void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
143{
144
145 struct FloatRates rates = *gyro;
146 /* unbias measurement */
147 RATES_SUB(rates, ahrs_fc.gyro_bias);
148
149#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
150 const float alpha = 0.1;
151 FLOAT_RATES_LIN_CMB(ahrs_fc.body_rate, ahrs_fc.body_rate, (1. - alpha), rates, alpha);
152#else
153 RATES_COPY(ahrs_fc.body_rate, rates);
154#endif
155
156 /* add correction */
157 struct FloatRates omega;
158 RATES_SUM(omega, rates, ahrs_fc.rate_correction);
159 /* and zeros it */
160 FLOAT_RATES_ZERO(ahrs_fc.rate_correction);
161
162#if AHRS_PROPAGATE_RMAT
163 float_rmat_integrate_fi(&ahrs_fc.ltp_to_body_rmat, &omega, dt);
164 float_rmat_reorthogonalize(&ahrs_fc.ltp_to_body_rmat);
165 float_quat_of_rmat(&ahrs_fc.ltp_to_body_quat, &ahrs_fc.ltp_to_body_rmat);
166#endif
167#if AHRS_PROPAGATE_QUAT
168 float_quat_integrate(&ahrs_fc.ltp_to_body_quat, &omega, dt);
169 float_quat_normalize(&ahrs_fc.ltp_to_body_quat);
170 float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
171#endif
172
173 // increase accel and mag propagation counters
174 ahrs_fc.accel_cnt++;
175 ahrs_fc.mag_cnt++;
176}
177
178void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
179{
180 // check if we had at least one propagation since last update
181 if (ahrs_fc.accel_cnt == 0) {
182 return;
183 }
184
185 /* last column of roation matrix = ltp z-axis in body-frame */
186 struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 2),
187 RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 1, 2),
188 RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 2)
189 };
190
191 struct FloatVect3 imu_accel = *accel;
192 struct FloatVect3 residual;
193 struct FloatVect3 pseudo_gravity_measurement;
194
195 if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) {
196 /*
197 * centrifugal acceleration in body frame
198 * a_c_body = omega x (omega x r)
199 * (omega x r) = tangential velocity in body frame
200 * a_c_body = omega x vel_tangential_body
201 * assumption: tangential velocity only along body x-axis (or negative z-axis)
202 */
203 const struct FloatVect3 vel_tangential_body =
204#if AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION
205 /* AHRS_GRAVITY_UPDATE_COORDINATED_TURN assumes the GPS speed is in the X axis direction.
206 * Quadshot, DelftaCopter and other hybrids can have the GPS speed in the negative Z direction
207 */
208 {0.0, 0.0, -ahrs_fc.ltp_vel_norm);
209#else
210 /* assume tangential velocity along body x-axis */
211 {ahrs_fc.ltp_vel_norm, 0.0, 0.0};
212#endif
213 struct FloatVect3 acc_c_body;
214 VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_fc.body_rate, vel_tangential_body);
215
216 /* centrifugal acceleration subtract it from imu measurement to get a corrected measurement of the gravity vector */
217 VECT3_DIFF(pseudo_gravity_measurement, imu_accel, acc_c_body);
218
219 } else {
220 VECT3_COPY(pseudo_gravity_measurement, imu_accel);
221 }
222
223 VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
224
225 /* FIR filtered pseudo_gravity_measurement */
226#define FIR_FILTER_SIZE 8
227 static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
228 VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
229 VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
230 VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
231
232 if (ahrs_fc.gravity_heuristic_factor) {
233 /* heuristic on acceleration (gravity estimate) norm */
234 /* Factor how strongly to change the weight.
235 * e.g. for gravity_heuristic_factor 30:
236 * <0.66G = 0, 1G = 1.0, >1.33G = 0
237 */
238
239 const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81;
240 ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
241 Bound(ahrs_fc.weight, 0.15, 1.0);
242 } else {
243 ahrs_fc.weight = 1.0;
244 }
245
246 /* Complementary filter proportional gain.
247 * Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt
248 * with ahrs_fc.accel_cnt beeing the number of propagations since last update
249 */
250 const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega *
251 ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81;
252 RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain);
253
254 // reset accel propagation counter
255 ahrs_fc.accel_cnt = 0;
256
257 /* Complementary filter integral gain
258 * Correct the gyro bias.
259 * Ki = (omega*weight)^2 * dt
260 */
261 const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega *
262 ahrs_fc.weight * ahrs_fc.weight * dt / 9.81;
263 RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain);
264
265 /* FIXME: saturate bias */
266}
267
268
269void ahrs_fc_update_mag(struct FloatVect3 *mag __attribute__((unused)), float dt __attribute__((unused)))
270{
271#if USE_MAGNETOMETER
272 // check if we had at least one propagation since last update
273 if (ahrs_fc.mag_cnt == 0) {
274 return;
275 }
276#if AHRS_MAG_UPDATE_ALL_AXES
278#else
279 ahrs_fc_update_mag_2d(mag, dt);
280#endif
281 // reset mag propagation counter
282 ahrs_fc.mag_cnt = 0;
283#endif
284}
285
286void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt)
287{
288
289 struct FloatVect3 expected_imu;
290 float_rmat_vmult(&expected_imu, &ahrs_fc.ltp_to_body_rmat, &ahrs_fc.mag_h);
291
292 struct FloatVect3 measured_imu = *mag;
293 struct FloatVect3 residual_imu;
294 VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
295 // DISPLAY_FLOAT_VECT3("# expected", expected_imu);
296 // DISPLAY_FLOAT_VECT3("# measured", measured_imu);
297 // DISPLAY_FLOAT_VECT3("# residual", residual);
298
299 /* Complementary filter proportional gain.
300 * Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt
301 * with ahrs_fc.mag_cnt beeing the number of propagations since last update
302 */
303
304 const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt;
305 RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain);
306
307 /* Complementary filter integral gain
308 * Correct the gyro bias.
309 * Ki = (omega*weight)^2 * dt
310 */
311 const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt;
312 RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain);
313
314}
315
316void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
317{
318
319 struct FloatVect2 expected_ltp;
320 VECT2_COPY(expected_ltp, ahrs_fc.mag_h);
321 // normalize expected ltp in 2D (x,y)
322 float_vect2_normalize(&expected_ltp);
323
324 struct FloatVect3 measured_imu = *mag;
325 struct FloatVect3 measured_ltp;
326 float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_body_rmat, &measured_imu);
327
328 struct FloatVect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
329
330 // normalize measured ltp in 2D (x,y)
331 float_vect2_normalize(&measured_ltp_2d);
332
333 struct FloatVect3 residual_ltp = {
334 0,
335 0,
336 measured_ltp_2d.x *expected_ltp.y - measured_ltp_2d.y * expected_ltp.x
337 };
338
339 // printf("res : %f\n", residual_ltp.z);
340
341 struct FloatVect3 residual_imu;
342 float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_body_rmat, &residual_ltp);
343
344
345 /* Complementary filter proportional gain.
346 * Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt
347 * with ahrs_fc.mag_cnt beeing the number of propagations since last update
348 */
349 const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt;
350 RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain);
351
352 /* Complementary filter integral gain
353 * Correct the gyro bias.
354 * Ki = (omega*weight)^2 * dt
355 */
356 const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt;
357 RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain);
358}
359
360
362{
363
364 /* project mag on local tangeant plane */
365 struct FloatEulers ltp_to_body_euler;
366 float_eulers_of_rmat(&ltp_to_body_euler, &ahrs_fc.ltp_to_body_rmat);
367
368 const float cphi = cosf(ltp_to_body_euler.phi);
369 const float sphi = sinf(ltp_to_body_euler.phi);
370 const float ctheta = cosf(ltp_to_body_euler.theta);
371 const float stheta = sinf(ltp_to_body_euler.theta);
372 const float mn = ctheta * mag->x + sphi * stheta * mag->y + cphi * stheta * mag->z;
373 const float me = 0. * mag->x + cphi * mag->y - sphi * mag->z;
374
375 const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 0) * me +
376 RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 1, 0) * mn;
377 const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 0),
378 RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 1),
379 RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 2)
380 };
381 const float mag_rate_update_gain = 2.5;
382 RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm));
383 const float mag_bias_update_gain = -mag_rate_update_gain * 1e-4;
384 RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm));
385
386}
387
388void ahrs_fc_update_gps(struct GpsState *gps_s __attribute__((unused)))
389{
390#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
391 if (gps_s->fix >= GPS_FIX_3D) {
392 ahrs_fc.ltp_vel_norm = gps_s->speed_3d / 100.;
393 ahrs_fc.ltp_vel_norm_valid = true;
394 } else {
395 ahrs_fc.ltp_vel_norm_valid = false;
396 }
397#endif
398
399#if AHRS_USE_GPS_HEADING && USE_GPS
400 //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg
401 if (gps_s->fix >= GPS_FIX_3D && gps_s->gspeed >= 500 &&
402 gps_s->cacc <= RadOfDeg(10 * 1e7)) {
403
404 // gps_s->course is in rad * 1e7, we need it in rad
405 float course = gps_s->course / 1e7;
406
407 if (ahrs_fc.heading_aligned) {
408 /* the assumption here is that there is no side-slip, so heading=course */
410 } else {
411 /* hard reset the heading if this is the first measurement */
413 }
414 }
415#endif
416}
417
418
420{
421
423
424 // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
425 // we only consider x and y
426 struct FloatVect2 expected_ltp = {
427 RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 0),
428 RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 1)
429 };
430
431 // expected_heading cross measured_heading
432 struct FloatVect3 residual_ltp = {
433 0,
434 0,
435 expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
436 };
437
438 struct FloatVect3 residual_imu;
439 float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_body_rmat, &residual_ltp);
440
441 const float heading_rate_update_gain = 2.5;
442 RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain);
443
444 float heading_bias_update_gain;
445 /* crude attempt to only update bias if deviation is small
446 * e.g. needed when you only have gps providing heading
447 * and the inital heading is totally different from
448 * the gps course information you get once you have a gps fix.
449 * Otherwise the bias will be falsely "corrected".
450 */
451 if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) {
452 heading_bias_update_gain = -heading_rate_update_gain * 1e-4;
453 } else {
454 heading_bias_update_gain = 0.0;
455 }
456 RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain);
457}
458
459
461{
463
464 /* quaternion representing only the heading rotation from ltp to body */
465 struct FloatQuat q_h_new;
466 q_h_new.qx = 0.0;
467 q_h_new.qy = 0.0;
468 q_h_new.qz = sinf(heading / 2.0);
469 q_h_new.qi = cosf(heading / 2.0);
470
471 struct FloatQuat ltp_to_body_quat;
472 QUAT_COPY(ltp_to_body_quat, ahrs_fc.ltp_to_body_quat);
473
474 /* quaternion representing current heading only */
475 struct FloatQuat q_h = ltp_to_body_quat;
476 q_h.qx = 0.;
477 q_h.qy = 0.;
479
480 /* quaternion representing rotation from current to new heading */
481 struct FloatQuat q_c;
482 float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
483
484 /* correct current heading in body frame */
485 float_quat_comp_norm_shortest(&ahrs_fc.ltp_to_body_quat, &q_c, &ltp_to_body_quat);
486
487 /* compute ltp to imu rotation quaternion and matrix */
488 float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
489
490 ahrs_fc.heading_aligned = true;
491}
#define AHRS_MAG_OMEGA
struct AhrsFloatCmpl ahrs_fc
void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
#define AHRS_MAG_ZETA
void ahrs_fc_init(void)
void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt)
void ahrs_fc_update_mag(struct FloatVect3 *mag, float dt)
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void ahrs_fc_update_mag_2d_dumb(struct FloatVect3 *mag)
void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
void ahrs_fc_realign_heading(float heading)
Hard reset yaw to a heading.
#define FIR_FILTER_SIZE
#define AHRS_ACCEL_OMEGA
void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
void ahrs_fc_update_gps(struct GpsState *gps_s)
#define AHRS_ACCEL_ZETA
void ahrs_fc_update_heading(float heading)
Update yaw based on a heading measurement.
Complementary filter in float to estimate the attitude, heading and gyro bias.
@ AHRS_FC_UNINIT
@ AHRS_FC_RUNNING
Utility functions for floating point AHRS implementations.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
static int16_t course[3]
static uint16_t c2
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
data structure for GPS information
Definition gps.h:90
float phi
in radians
float theta
in radians
static void float_quat_normalize(struct FloatQuat *q)
#define FLOAT_ANGLE_NORMALIZE(_a)
static float float_vect3_norm(const struct FloatVect3 *v)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
float float_rmat_reorthogonalize(struct FloatRMat *rm)
static void float_rmat_identity(struct FloatRMat *rm)
initialises a rotation matrix to identity
void float_quat_of_rmat(struct FloatQuat *q, const struct FloatRMat *rm)
Quaternion from rotation matrix.
void float_rmat_integrate_fi(struct FloatRMat *rm, const struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_eulers_of_rmat(struct FloatEulers *e, const struct FloatRMat *rm)
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
void float_quat_integrate(struct FloatQuat *q, const struct FloatRates *omega, const float dt)
in place quaternion integration with constant rotational velocity
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
void float_rmat_of_quat(struct FloatRMat *rm, const struct FloatQuat *q)
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define FLOAT_RATES_ZERO(_r)
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
euler angles
Roation quaternion.
angular rates
#define VECT3_SDIV(_vo, _vi, _s)
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
#define RATES_COPY(_a, _b)
#define RATES_ADD_SCALED_VECT(_ro, _v, _s)
#define VECT2_COPY(_a, _b)
#define RATES_SUB(_a, _b)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define VECT3_SMUL(_vo, _vi, _s)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define QUAT_COPY(_qo, _qi)
#define RATES_SUM(_c, _a, _b)
#define RMAT_ELMT(_rm, _row, _col)
#define VECT3_COPY(_a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_ADD(_a, _b)
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
Simple matrix helper macros.
float alpha
Definition textons.c:133
float heading
Definition wedgebug.c:258