Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 04:45:37 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[math] more x_integrate_fi functions

Browse Source
This commit is contained in:
Felix Ruess
2014-09-11 12:18:54 +02:00
parent 63d67a322f
commit aa10dbe2a8
2 changed files with 34 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra_float.c
+23 -1
View File
@@ -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;
sw/airborne/math/pprz_algebra_float.h
+11 -16
View File
@@ -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))
/*