diff --git a/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c b/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c
index 92f992501f..dbfab46dea 100644
--- a/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c
+++ b/sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c
@@ -53,6 +53,12 @@ void booz_ahrs_init(void) {
   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 @@ void booz_ahrs_update_mag(void) {
   //  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);
 
 }
 
diff --git a/sw/airborne/booz/booz_ahrs.c b/sw/airborne/booz/booz_ahrs.c
index 7083200be3..00445295c4 100644
--- a/sw/airborne/booz/booz_ahrs.c
+++ b/sw/airborne/booz/booz_ahrs.c
@@ -26,3 +26,6 @@
 
 struct BoozAhrs booz_ahrs;
 struct BoozAhrsFloat booz_ahrs_float;
+
+float booz_ahrs_mag_offset;
+
diff --git a/sw/airborne/booz/booz_ahrs.h b/sw/airborne/booz/booz_ahrs.h
index c9fd54b9ff..cd5fbbdd74 100644
--- a/sw/airborne/booz/booz_ahrs.h
+++ b/sw/airborne/booz/booz_ahrs.h
@@ -64,6 +64,8 @@ struct BoozAhrsFloat {
 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); \