[math] more x_integrate_fi functions

paparazzi · Sep 11, 2014 · aa10dbe · aa10dbe
1 parent 63d67a3
commit aa10dbe
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 17 deletions.
diff --git 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
@@ -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))
 
 
 /*