diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index d535edefe6..706928e225 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -189,8 +189,7 @@ static inline void UNUSED nav_advance_carrot(void) {
   /* saturate it */
   VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
 
-  int32_t dist_to_waypoint;
-  INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
+  int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
 
   if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
     VECT2_COPY(navigation_carrot, navigation_target);
@@ -311,7 +310,7 @@ bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int1
    * distance with half metric precision (6.25 cm)
    */
   INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2);
-  INT32_VECT2_NORM(dist_to_point, diff);
+  dist_to_point = int32_vect2_norm(&diff);
 
   /* return TRUE if we have arrived */
   if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2))
@@ -331,7 +330,7 @@ bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int1
 
 bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) {
   uint16_t time_at_wp;
-  int32_t dist_to_point;
+  uint32_t dist_to_point;
   static uint16_t wp_entry_time = 0;
   static bool_t wp_reached = FALSE;
   static struct EnuCoor_i wp_last = { 0, 0, 0 };
@@ -343,7 +342,7 @@ bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) {
   }
   VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
   INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2);
-  INT32_VECT2_NORM(dist_to_point, diff);
+  dist_to_point = int32_vect2_norm(&diff);
   if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2)){
     if (!wp_reached) {
       wp_reached = TRUE;
@@ -510,7 +509,7 @@ bool_t nav_set_heading_towards(float x, float y) {
   struct FloatVect2 pos_diff;
   VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f());
   // don't change heading if closer than 0.5m to target
-  if (FLOAT_VECT2_NORM2(pos_diff) > 0.25) {
+  if (VECT2_NORM2(pos_diff) > 0.25) {
     float heading_f = atan2f(pos_diff.x, pos_diff.y);
     nav_heading = ANGLE_BFP_OF_REAL(heading_f);
   }
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
index 608952b88d..396c295247 100644
--- a/sw/airborne/math/pprz_algebra.h
+++ b/sw/airborne/math/pprz_algebra.h
@@ -106,6 +106,8 @@ extern "C" {
 /* _vo=v1*v2 */
 #define VECT2_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y)
 
+#define VECT2_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y)
+
 /*
  * Dimension 3 vectors
  */
@@ -238,6 +240,8 @@ extern "C" {
 
 #define VECT3_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
 
+#define VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
+
 #define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) {    \
     (_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y;    \
     (_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z;    \
diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c
index 41a7926f8d..0430345177 100644
--- a/sw/airborne/math/pprz_algebra_float.c
+++ b/sw/airborne/math/pprz_algebra_float.c
@@ -213,11 +213,11 @@ float float_rmat_reorthogonalize(struct FloatRMat* rm)
   VECT3_SUM_SCALED(r1_t,  r1, r0, _err);
   struct FloatVect3 r2_t;
   VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
-  float s = renorm_factor(FLOAT_VECT3_NORM2(r0_t));
+  float s = renorm_factor(VECT3_NORM2(r0_t));
   MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
-  s = renorm_factor(FLOAT_VECT3_NORM2(r1_t));
+  s = renorm_factor(VECT3_NORM2(r1_t));
   MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
-  s = renorm_factor(FLOAT_VECT3_NORM2(r2_t));
+  s = renorm_factor(VECT3_NORM2(r2_t));
   MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
 
   return _err;
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index a60fd23033..9f51e08fa0 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -109,16 +109,30 @@ struct FloatRates {
 
 #define FLOAT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0., 0.)
 
-#define FLOAT_VECT2_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y)
+/* macros also usable if _v is not a FloatVect2, but a different struct with x,y members */
+#define FLOAT_VECT2_NORM(_v) sqrtf(VECT2_NORM2(_v))
 
-#define FLOAT_VECT2_NORM(_n, _v) {               \
-    _n = sqrtf(FLOAT_VECT2_NORM2(_v));           \
-  }
+static inline float float_vect2_norm2(struct FloatVect2* v)
+{
+  return v->x * v->x + v->y * v->y;
+}
 
-#define FLOAT_VECT2_NORMALIZE(_v) {             \
-    const float n = sqrtf(FLOAT_VECT2_NORM2(_v)); \
-    VECT2_SMUL(_v, _v, 1./n);             \
+static inline float float_vect2_norm(struct FloatVect2* v)
+{
+  return sqrtf(float_vect2_norm2(v));
+}
+
+/** normalize 2D vector in place */
+static inline void float_vect2_normalize(struct FloatVect2* v)
+{
+  const float n = float_vect2_norm(v);
+  if (n > 0) {
+    v->x /= n;
+    v->y /= n;
   }
+}
+
+#define FLOAT_VECT2_NORMALIZE(_v) float_vect2_normalize(&(_v))
 
 
 /*
@@ -127,14 +141,33 @@ struct FloatRates {
 
 #define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
 
-#define FLOAT_VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
+/* macros also usable if _v is not a FloatVect3, but a different struct with x,y,z members */
+#define FLOAT_VECT3_NORM(_v) sqrtf(VECT3_NORM2(_v))
 
-#define FLOAT_VECT3_NORM(_v) (sqrtf(FLOAT_VECT3_NORM2(_v)))
+static inline float float_vect3_norm2(struct FloatVect3* v)
+{
+  return v->x * v->x + v->y * v->y + v->z * v->z;
+}
 
-#define FLOAT_VECT3_NORMALIZE(_v) {     \
-    const float n = FLOAT_VECT3_NORM(_v);   \
-    VECT3_SMUL(_v, _v, 1./n);     \
+static inline float float_vect3_norm(struct FloatVect3* v)
+{
+  return sqrtf(float_vect3_norm2(v));
+}
+
+/** normalize 3D vector in place */
+static inline void float_vect3_normalize(struct FloatVect3* v)
+{
+  const float n = float_vect3_norm(v);
+  if (n > 0) {
+    v->x /= n;
+    v->y /= n;
+    v->z /= n;
   }
+}
+
+#define FLOAT_VECT3_NORMALIZE(_v) float_vect3_norm(&(_v))
+
+
 
 #define FLOAT_RATES_ZERO(_r) {          \
     RATES_ASSIGN(_r, 0., 0., 0.);       \
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 901a51ce6a..54fdd1a65f 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -231,16 +231,30 @@ extern int32_t int32_sqrt(int32_t in);
 
 #define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
 
-#define INT32_VECT2_NORM(n, v) {            \
-    int32_t n2 = (v).x*(v).x + (v).y*(v).y; \
-    n = int32_sqrt(n2);                  \
-  }
+/* macros also usable if _v is not a Int32Vect2, but a different struct with x,y members */
+#define INT32_VECT2_NORM(_v) int32_sqrt(VECT2_NORM2(_v))
 
-#define INT32_VECT2_NORMALIZE(_v,_frac) {       \
-    int32_t n;                \
-    INT32_VECT2_NORM(n, _v);            \
-    INT32_VECT2_SCALE_2(_v, _v, BFP_OF_REAL((1.),_frac) , n);   \
+static inline uint32_t int32_vect2_norm2(struct Int32Vect2* v)
+{
+  return v->x * v->x + v->y * v->y;
+}
+
+static inline uint32_t int32_vect2_norm(struct Int32Vect2* v)
+{
+  return int32_sqrt(int32_vect2_norm2(v));
+}
+
+static inline void int32_vect2_normalize(struct Int32Vect2* v, uint8_t frac)
+{
+  const uint32_t f = BFP_OF_REAL((1.), frac);
+  const uint32_t n = int32_vect2_norm(v);
+  if (n > 0) {
+    v->x = v->x * f / n;
+    v->y = v->y * f / n;
   }
+}
+
+#define INT32_VECT2_NORMALIZE(_v,_frac) int32_vect2_normalize(&(_v), _frac)
 
 
 #define INT32_VECT2_RSHIFT(_o, _i, _r) { \
@@ -271,10 +285,7 @@ extern int32_t int32_sqrt(int32_t in);
     (_a).z = ((_b).z * (_num)) / (_den);        \
   }
 
-#define INT32_VECT3_NORM(n, v) {                \
-    int32_t n2 = (v).x*(v).x + (v).y*(v).y + (v).z*(v).z;   \
-    n = int32_sqrt(n2);                  \
-  }
+#define INT32_VECT3_NORM(_v) int32_sqrt(VECT3_NORM2(_v))
 
 #define INT32_VECT3_RSHIFT(_o, _i, _r) { \
     (_o).x = ((_i).x >> (_r));       \
diff --git a/sw/airborne/modules/digital_cam/hackhd.c b/sw/airborne/modules/digital_cam/hackhd.c
index 593661d51e..3125d43415 100644
--- a/sw/airborne/modules/digital_cam/hackhd.c
+++ b/sw/airborne/modules/digital_cam/hackhd.c
@@ -229,7 +229,7 @@ void hackhd_autoshoot(void) {
     struct FloatVect2 d_pos;
     d_pos.x = pos.x - hackhd.last_shot_pos.x;
     d_pos.y = pos.y - hackhd.last_shot_pos.y;
-    if (FLOAT_VECT2_NORM2(d_pos) > (HACKHD_AUTOSHOOT_DIST*HACKHD_AUTOSHOOT_DIST)
+    if (VECT2_NORM2(d_pos) > (HACKHD_AUTOSHOOT_DIST*HACKHD_AUTOSHOOT_DIST)
         || hackhd.status == HACKHD_AUTOSHOOT_START) {
 #endif
       // take a picture
diff --git a/sw/airborne/modules/mission/mission_rotorcraft_nav.c b/sw/airborne/modules/mission/mission_rotorcraft_nav.c
index 6dc9fa9993..3a324efbca 100644
--- a/sw/airborne/modules/mission/mission_rotorcraft_nav.c
+++ b/sw/airborne/modules/mission/mission_rotorcraft_nav.c
@@ -56,8 +56,7 @@ bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) {
   VECT2_DIFF(vect_from_home, tmp_enu_point, home);
   //Saturate the mission wp not to overflow max_dist_from_home
   //including a buffer zone before limits
-  float dist_to_home;
-  FLOAT_VECT2_NORM(dist_to_home, vect_from_home);
+  float dist_to_home = float_vect2_norm(&vect_from_home);
   dist_to_home += BUFFER_ZONE_DIST;
   if (dist_to_home > MAX_DIST_FROM_HOME) {
     VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
diff --git a/sw/airborne/modules/nav/nav_spiral.c b/sw/airborne/modules/nav/nav_spiral.c
index 62624fd6ca..65c2607919 100644
--- a/sw/airborne/modules/nav/nav_spiral.c
+++ b/sw/airborne/modules/nav/nav_spiral.c
@@ -61,7 +61,7 @@ bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start,
   struct FloatVect2 edge;
   VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
 
-  FLOAT_VECT2_NORM(nav_spiral.radius, edge);
+  nav_spiral.radius = float_vect2_norm(&edge);
 
   // get a copy of the current position
   struct EnuCoor_f pos_enu;
@@ -132,7 +132,7 @@ bool_t nav_spiral_run(void)
       //DistanceStartEstim = |Starting position angular - current positon|
       struct FloatVect2 pos_diff;
       VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
-      FLOAT_VECT2_NORM(DistanceStartEstim, pos_diff);
+      DistanceStartEstim = float_vect2_norm(&pos_diff);
       CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start)));
       if (CircleAlpha >= nav_spiral.alpha_limit) {
         VECT2_COPY(nav_spiral.last_circle, pos_enu);
diff --git a/sw/airborne/state.c b/sw/airborne/state.c
index c00dc4a75a..6d4845a38c 100644
--- a/sw/airborne/state.c
+++ b/sw/airborne/state.c
@@ -767,7 +767,7 @@ void stateCalcHorizontalSpeedNorm_i(void) {
     INT32_SQRT(state.h_speed_norm_i, n2);
   }
   else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
-    FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
+    state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
     SetBit(state.speed_status, SPEED_HNORM_F);
     state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
   }
@@ -777,7 +777,7 @@ void stateCalcHorizontalSpeedNorm_i(void) {
     INT32_SQRT(state.h_speed_norm_i, n2);
   }
   else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
-    FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
+    state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
     SetBit(state.speed_status, SPEED_HNORM_F);
     state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
   }
@@ -792,7 +792,7 @@ void stateCalcHorizontalSpeedNorm_i(void) {
   else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
     ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
     SetBit(state.speed_status, SPEED_NED_F);
-    FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
+    state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
     SetBit(state.speed_status, SPEED_HNORM_F);
     state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
   }
@@ -982,20 +982,20 @@ void stateCalcHorizontalSpeedNorm_f(void) {
     state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
   }
   else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
-    FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
+    state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
   }
   else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
-    FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
+    state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
   }
   else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
     SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
     SetBit(state.speed_status, SPEED_NED_F);
-    FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
+    state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
   }
   else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
     SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
     SetBit(state.speed_status, SPEED_ENU_F);
-    FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
+    state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
   }
   /* set bit to indicate this representation is computed */
   SetBit(state.speed_status, SPEED_HNORM_F);
diff --git a/sw/airborne/test/pprz_algebra_print.h b/sw/airborne/test/pprz_algebra_print.h
index 1d06d01e86..5a8c0e6bb3 100644
--- a/sw/airborne/test/pprz_algebra_print.h
+++ b/sw/airborne/test/pprz_algebra_print.h
@@ -65,8 +65,7 @@
 
 
 #define DISPLAY_INT32_VECT3(text, _v) {					\
-    int32_t norm;							\
-    INT32_VECT3_NORM(norm, _v);						\
+    int32_t norm = INT32_VECT3_NORM(_v);						\
     printf("%s %d %d %d (%d)\n",text,  (_v).x, (_v).y, (_v).z, norm);	\
   }