diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h
index d88dbff306..df3cec46fc 100644
--- a/sw/airborne/math/pprz_algebra_double.h
+++ b/sw/airborne/math/pprz_algebra_double.h
@@ -93,6 +93,39 @@ struct DoubleRates {
     (_vout).z = rint((_vin).z);         \
   }
 
+static inline double double_vect3_norm(struct DoubleVect3* v)
+{
+  return sqrt(VECT3_NORM2(*v));
+}
+
+/** normalize 3D vector in place */
+static inline void double_vect3_normalize(struct DoubleVect3* v)
+{
+  const double n = double_vect3_norm(v);
+  if (n > 0) {
+    v->x /= n;
+    v->y /= n;
+    v->z /= n;
+  }
+}
+
+static inline double double_quat_norm(struct DoubleQuat* q)
+{
+  return sqrt(SQUARE(q->qi) + SQUARE(q->qx) +  SQUARE(q->qy) + SQUARE(q->qz));
+}
+
+
+static inline void double_quat_normalize(struct DoubleQuat* q)
+{
+  double qnorm = double_quat_norm(q);
+  if (qnorm > FLT_MIN) {
+    q->qi = q->qi / qnorm;
+    q->qx = q->qx / qnorm;
+    q->qy = q->qy / qnorm;
+    q->qz = q->qz / qnorm;
+  }
+}
+
 extern void double_rmat_of_eulers_321(struct DoubleRMat* rm, struct DoubleEulers* e);
 extern void double_quat_of_eulers(struct DoubleQuat* q, struct DoubleEulers* e);
 extern void double_eulers_of_quat(struct DoubleEulers* e, struct DoubleQuat* q);
diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c
index 3f9ee5081c..f5760118c9 100644
--- a/sw/airborne/modules/geo_mag/geo_mag.c
+++ b/sw/airborne/modules/geo_mag/geo_mag.c
@@ -34,15 +34,15 @@
 #include "subsystems/ahrs.h"
 
 bool_t geo_mag_calc_flag;
-struct GeoMagVect geo_mag_vect;
+struct GeoMag geo_mag;
 
 void geo_mag_init(void) {
   geo_mag_calc_flag = FALSE;
-  geo_mag_vect.ready = FALSE;
+  geo_mag.ready = FALSE;
 }
 
 void geo_mag_periodic(void) {
-  if (!geo_mag_vect.ready && gps.fix == GPS_FIX_3D && kill_throttle)
+  if (!geo_mag.ready && gps.fix == GPS_FIX_3D && kill_throttle)
     geo_mag_calc_flag = TRUE;
 }
 
@@ -66,19 +66,19 @@ void geo_mag_event(void) {
     nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
     // Calculates absolute magnet fields
     mag_calc(1, latitude, longitude, alt, nmax, gha,
-             &geo_mag_vect.x, &geo_mag_vect.y, &geo_mag_vect.z,
+             &geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z,
              IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
-    FLOAT_VECT3_NORMALIZE(geo_mag_vect);
+    double_vect3_normalize(&geo_mag.vect);
 
     // copy to ahrs
 #ifdef AHRS_FLOAT
-    VECT3_COPY(ahrs_impl.mag_h, geo_mag_vect);
+    VECT3_COPY(ahrs_impl.mag_h, geo_mag.vect);
 #else
     // convert to MAG_BFP and copy to ahrs
-    VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag_vect.x), MAG_BFP_OF_REAL(geo_mag_vect.y), MAG_BFP_OF_REAL(geo_mag_vect.z));
+    VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
 #endif
 
-    geo_mag_vect.ready = TRUE;
+    geo_mag.ready = TRUE;
   }
   geo_mag_calc_flag = FALSE;
 }
diff --git a/sw/airborne/modules/geo_mag/geo_mag.h b/sw/airborne/modules/geo_mag/geo_mag.h
index b752d785e7..08eb44bb4f 100644
--- a/sw/airborne/modules/geo_mag/geo_mag.h
+++ b/sw/airborne/modules/geo_mag/geo_mag.h
@@ -29,11 +29,10 @@
 #define GEO_MAG_H
 
 #include "std.h"
+#include "math/pprz_algebra_double.h"
 
-struct GeoMagVect {
-  double x;
-  double y;
-  double z;
+struct GeoMag {
+  struct DoubleVect3 vect;
   bool_t ready;
 };
 
@@ -41,6 +40,6 @@ extern void geo_mag_init(void);
 extern void geo_mag_periodic(void);
 extern void geo_mag_event(void);
 
-extern struct GeoMagVect geo_mag_vect;
+extern struct GeoMag geo_mag;
 
 #endif
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp
index abec4d1905..47ad43fa52 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.cpp
+++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp
@@ -506,7 +506,7 @@ PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated l
   mag_calc(1, latitude, longitude, alt, nmax, gha,
            &fdm.ltp_h.x, &fdm.ltp_h.y, &fdm.ltp_h.z,
            IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
-  FLOAT_VECT3_NORMALIZE(fdm.ltp_h);
+  double_vect3_normalize(&fdm.ltp_h);
   printf("normalized magnetic field: %.4f %.4f %.4f\n", fdm.ltp_h.x, fdm.ltp_h.y, fdm.ltp_h.z);
 #endif