[math] some more math macros

paparazzi · Jul 12, 2012 · 7bc4ba3 · 7bc4ba3
1 parent 8aa6297
commit 7bc4ba3
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 5 deletions.
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
@@ -628,6 +628,18 @@
     (_ri).r = RATE_BFP_OF_REAL((_rf).r);		\
   }
 
+#define SPEEDS_FLOAT_OF_BFP(_ef, _ei) {			\
+    (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x);		\
+    (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y);		\
+    (_ef).z = SPEED_FLOAT_OF_BFP((_ei).z);		\
+  }
+
+#define SPEEDS_BFP_OF_REAL(_ef, _ei) {			\
+    (_ef).x = SPEED_BFP_OF_REAL((_ei).x);		\
+    (_ef).y = SPEED_BFP_OF_REAL((_ei).y);		\
+    (_ef).z = SPEED_BFP_OF_REAL((_ei).z);		\
+  }
+
 #define ACCELS_FLOAT_OF_BFP(_ef, _ei) {			\
     (_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x);		\
     (_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y);		\

diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
@@ -123,6 +123,10 @@ struct FloatRates {
 /* _vo = _vi * _s */
 #define FLOAT_VECT2_SMUL(_vo, _vi, _s) VECT2_SMUL(_vo, _vi, _s)
 
+#define FLOAT_VECT2_NORM(n, v) {               \
+    n = sqrtf((v).x*(v).x + (v).y*(v).y);      \
+  }
+
 
 /*
  * Dimension 3 Vectors

diff --git a/sw/airborne/math/pprz_geodetic.h b/sw/airborne/math/pprz_geodetic.h
@@ -20,4 +20,11 @@
     (_pos1).alt = (_pos2).alt;			\
 }
 
+#define LTP_DEF_COPY(_def1,_def2){                              \
+    LLA_COPY((_def1).lla, (_def2).lla);                         \
+    VECT3_COPY((_def1).ecef, (_def2).ecef);                     \
+    RMAT_COPY((_def1).ltp_of_ecef, (_def2).ltp_of_ecef);        \
+    (_def1).hmsl = (_def2).hmsl;                                \
+  }
+
 #endif /* PPRZ_GEODETIC_H */
diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h
@@ -123,13 +123,15 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st
 #define EM7RAD_OF_RAD(_r) ((_r)*1e7)
 #define RAD_OF_EM7RAD(_r) ((_r)/1e7)
 
-#define INT32_VECT3_ENU_OF_NED(_o, _i) {		\
-    (_o).x = (_i).y;                            \
-    (_o).y = (_i).x;                            \
-    (_o).z = -(_i).z;                           \
+#define VECT3_ENU_OF_NED(_o, _i) {		\
+    (_o).x = (_i).y;                    \
+    (_o).y = (_i).x;                    \
+    (_o).z = -(_i).z;                   \
   }
 
-#define INT32_VECT3_NED_OF_ENU(_o, _i) INT32_VECT3_ENU_OF_NED(_o,_i)
+#define VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
+#define INT32_VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
+#define INT32_VECT3_ENU_OF_NED(_o, _i) VECT3_ENU_OF_NED(_o,_i)
 
 #define ECEF_BFP_OF_REAL(_o, _i) {          \
     (_o).x = (int32_t)CM_OF_M((_i).x);      \