[modules] geo_mag: directly store mag_h as fixedpoint instead of conv…

…erting from double every time
paparazzi · Oct 5, 2012 · 065fe11 · 065fe11
1 parent 390812f
commit 065fe11
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 33 deletions.
diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c
@@ -11,7 +11,9 @@
 #include "math/pprz_algebra_double.h"
 #include "subsystems/gps.h"
 #include "autopilot.h"
-#include "subsystems/ahrs/ahrs_int_cmpl_quat.h" // in AHRS subsystem ahrs_h is DoubleVect3 variable
+
+// in int_cmpl_quat implementation, mag_h is an Int32Vect3 with INT32_MAG_FRAC
+#include "subsystems/ahrs/ahrs_int_cmpl_quat.h"
 
 bool_t geo_mag_calc_flag;
 struct GeoMagVect geo_mag_vect;
@@ -49,7 +51,10 @@ void geo_mag_event(void) {
              &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_COPY(ahrs_h, geo_mag_vect);
+
+    // 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));
+
     geo_mag_vect.ready = TRUE;
   }
   geo_mag_calc_flag = FALSE;

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
@@ -30,9 +30,6 @@
 #endif
 #include "math/pprz_trig_int.h"
 #include "math/pprz_algebra_int.h"
-#ifdef USE_GEO_MAG
-#include "math/pprz_algebra_double.h"
-#endif
 
 #include "generated/airframe.h"
 
@@ -60,9 +57,6 @@ static inline void compute_body_euler_and_rmat_from_quat(void);
 static inline void compute_imu_orientation(void);
 static inline void compute_body_orientation(void);
 
-#ifdef USE_GEO_MAG
-struct DoubleVect3 ahrs_h;
-#endif
 
 void ahrs_init(void) {
 
@@ -98,9 +92,7 @@ void ahrs_init(void) {
   ahrs_impl.use_gravity_heuristic = FALSE;
 #endif
 
-#ifdef USE_GEO_MAG
-  VECT3_ASSIGN(ahrs_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
-#endif
+  VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
 
 }
 
@@ -264,17 +256,9 @@ void ahrs_update_mag(void) {
 
 
 static inline void ahrs_update_mag_full(void) {
-#ifndef USE_GEO_MAG
-  const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
-                                          MAG_BFP_OF_REAL(AHRS_H_Y),
-                                          MAG_BFP_OF_REAL(AHRS_H_Z)};
-#else
-  const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(ahrs_h.x),
-                                          MAG_BFP_OF_REAL(ahrs_h.y),
-                                          MAG_BFP_OF_REAL(ahrs_h.z)};
-#endif
+
   struct Int32Vect3 expected_imu;
-  INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
+  INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, ahrs_impl.mag_h);
 
   struct Int32Vect3 residual;
   INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
@@ -296,21 +280,13 @@ static inline void ahrs_update_mag_full(void) {
 
 static inline void ahrs_update_mag_2d(void) {
 
-#ifndef USE_GEO_MAG
-  const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
-                                          MAG_BFP_OF_REAL(AHRS_H_Y)};
-#else
-  const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(ahrs_h.x),
-                                          MAG_BFP_OF_REAL(ahrs_h.y)};
-#endif
-
   struct Int32Vect3 measured_ltp;
   INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
 
   struct Int32Vect3 residual_ltp =
     { 0,
       0,
-      (measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
+      (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)};
 
   struct Int32Vect3 residual_imu;
   INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
@@ -31,6 +31,7 @@ struct AhrsIntCmpl {
   struct Int32Rates  rate_correction;
   struct Int64Quat   high_rez_quat;
   struct Int64Rates  high_rez_bias;
+  struct Int32Vect3  mag_h;
   int32_t ltp_vel_norm;
   bool_t ltp_vel_norm_valid;
   bool_t correct_gravity;
@@ -39,9 +40,6 @@ struct AhrsIntCmpl {
 };
 
 extern struct AhrsIntCmpl ahrs_impl;
-#ifdef USE_GEO_MAG
-extern struct DoubleVect3 ahrs_h;
-#endif
 
 
 /** Update yaw based on a heading measurement.