[paparazzi-commits] [5218] add a mag offset on ahrs (only with cmpl filter for now)

[Gautier Hattenberger]

Revision: 5218
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5218
Author:   gautier
Date:     2010-08-04 17:30:16 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
add a mag offset on ahrs (only with cmpl filter for now)

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c
    paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c
    paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h

Modified: paparazzi3/trunk/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c       
2010-08-04 17:15:12 UTC (rev 5217)
+++ paparazzi3/trunk/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c       
2010-08-04 17:30:16 UTC (rev 5218)
@@ -53,6 +53,12 @@
   booz2_face_reinj_1 = BOOZ2_FACE_REINJ_1;
 
   INT_EULERS_ZERO(booz2_face_uncorrected);
+
+#ifdef IMU_MAG_OFFSET
+  booz_ahrs_mag_offset = IMU_MAG_OFFSET;
+#else
+  booz_ahrs_mag_offset = 0.;
+#endif
 }
 
 void booz_ahrs_align(void) {
@@ -177,7 +183,7 @@
   //  sphi_ctheta * booz_imu.mag.y +
   //  cphi_ctheta * booz_imu.mag.z;
   float m_psi = -atan2(me, mn);
-  measurement.psi = ((m_psi)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
+  measurement.psi = ((m_psi - 
RadOfDeg(booz_ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
 
 }
 

Modified: paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c       2010-08-04 17:15:12 UTC 
(rev 5217)
+++ paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c       2010-08-04 17:30:16 UTC 
(rev 5218)
@@ -26,3 +26,6 @@
 
 struct BoozAhrs booz_ahrs;
 struct BoozAhrsFloat booz_ahrs_float;
+
+float booz_ahrs_mag_offset;
+

Modified: paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h       2010-08-04 17:15:12 UTC 
(rev 5217)
+++ paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h       2010-08-04 17:30:16 UTC 
(rev 5218)
@@ -64,6 +64,8 @@
 extern struct BoozAhrs booz_ahrs;
 extern struct BoozAhrsFloat booz_ahrs_float;
 
+extern float booz_ahrs_mag_offset;
+
 #define BOOZ_AHRS_FLOAT_OF_INT32() {                                           
         \
     QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, 
booz_ahrs.ltp_to_body_quat);     \
     EULERS_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_euler, 
booz_ahrs.ltp_to_body_euler); \