[math] add double versions for vect3/quat norm

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);

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;
 }

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 {
+struct GeoMag {
-  double x;
+  struct DoubleVect3 vect;
-  double y;
-  double z;
   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

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