From aa10dbe2a8de7bfb69911c999d03a0482d1bcfe8 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 11 Sep 2014 12:18:54 +0200 Subject: [PATCH] [math] more x_integrate_fi functions --- sw/airborne/math/pprz_algebra_float.c | 24 +++++++++++++++++++++++- sw/airborne/math/pprz_algebra_float.h | 27 +++++++++++---------------- 2 files changed, 34 insertions(+), 17 deletions(-) diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c index da8de688949..41a7926f8d7 100644 --- a/sw/airborne/math/pprz_algebra_float.c +++ b/sw/airborne/math/pprz_algebra_float.c @@ -26,10 +26,32 @@ #include "pprz_algebra_float.h" +/** in place first order integration of a 3D-vector */ +void float_vect3_integrate_fi(struct FloatVect3* vec, struct FloatVect3* dv, float dt) +{ + vec->x += dv->x * dt; + vec->y += dv->y * dt; + vec->z += dv->z * dt; +} + +/** in place first order integration of angular rates */ +void float_rates_integrate_fi(struct FloatRates* r, struct FloatRates* dr, float dt) +{ + r->p += dr->p * dt; + r->q += dr->q * dt; + r->r += dr->r * dt; +} + +void float_rates_of_euler_dot(struct FloatRates* r, struct FloatEulers* e, struct FloatEulers* edot) +{ + r->p = edot->phi - sinf(e->theta) * edot->psi; + r->q = cosf(e->phi) * edot->theta + sinf(e->phi) * cosf(e->theta) * edot->psi; + r->r = -sinf(e->phi) * edot->theta + cosf(e->phi) * cosf(e->theta) * edot->psi; +} + /** initialises a rotation matrix from unit vector axis and angle */ void float_rmat_of_axis_angle(struct FloatRMat* rm, struct FloatVect3* uv, float angle) { - const float ux2 = uv->x * uv->x; const float uy2 = uv->y * uv->y; const float uz2 = uv->z * uv->z; diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 67af172eb22..5b1ceecff53 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -136,12 +136,6 @@ struct FloatRates { VECT3_SMUL(_v, _v, 1./n); \ } -#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) { \ - (_vo).x += (_dv).x * (_dt); \ - (_vo).y += (_dv).y * (_dt); \ - (_vo).z += (_dv).z * (_dt); \ - } - #define FLOAT_RATES_ZERO(_r) { \ RATES_ASSIGN(_r, 0., 0., 0.); \ } @@ -162,18 +156,19 @@ struct FloatRates { } -#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) { \ - (_ra).p += (_racc).p * (_dt); \ - (_ra).q += (_racc).q * (_dt); \ - (_ra).r += (_racc).r * (_dt); \ - } +extern void float_vect3_integrate_fi(struct FloatVect3* vec, struct FloatVect3* dv, + float dt); -#define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) { \ - _ra.p = _ed.phi - sinf(_e.theta) *_ed.psi; \ - _ra.q = cosf(_e.phi)*_ed.theta + sinf(_e.phi)*cosf(_e.theta)*_ed.psi; \ - _ra.r = -sinf(_e.phi)*_ed.theta + cosf(_e.phi)*cosf(_e.theta)*_ed.psi; \ - } +extern void float_rates_integrate_fi(struct FloatRates* r, struct FloatRates* dr, + float dt); + +extern void float_rates_of_euler_dot(struct FloatRates* r, struct FloatEulers* e, + struct FloatEulers* edot); +/* defines for backwards compatibility */ +#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) float_vect3_integrate_fi(&(_vo), &(_dv), _dt) +#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) float_rates_integrate_fi(&(_ra), &(_racc), _dt) +#define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) float_rates_of_euler_dot(&(_ra), &(_e), &(_ed)) /*