diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c
index d1b26aa25d9..0c1c179a984 100644
--- a/sw/airborne/modules/sensors/mag_hmc58xx.c
+++ b/sw/airborne/modules/sensors/mag_hmc58xx.c
@@ -29,6 +29,7 @@
 #include "mcu_periph/uart.h"
 #include "pprzlink/messages.h"
 #include "subsystems/datalink/downlink.h"
+#include "generated/airframe.h"
 
 #ifndef HMC58XX_CHAN_X
 #define HMC58XX_CHAN_X 0
@@ -52,6 +53,13 @@
 #if MODULE_HMC58XX_UPDATE_AHRS
 #include "subsystems/imu.h"
 #include "subsystems/abi.h"
+
+#if defined HMC58XX_MAG_TO_IMU_PHI && defined HMC58XX_MAG_TO_IMU_THETA && defined HMC58XX_MAG_TO_IMU_PSI
+#define USE_MAG_TO_IMU 1
+static struct Int32RMat mag_to_imu; ///< rotation from mag to imu frame
+#else
+#define USE_MAG_TO_IMU 0
+#endif
 #endif
 
 struct Hmc58xx mag_hmc58xx;
@@ -59,6 +67,15 @@ struct Hmc58xx mag_hmc58xx;
 void mag_hmc58xx_module_init(void)
 {
   hmc58xx_init(&mag_hmc58xx, &(MAG_HMC58XX_I2C_DEV), HMC58XX_ADDR);
+
+#if MODULE_HMC58XX_UPDATE_AHRS && USE_MAG_TO_IMU
+  struct Int32Eulers mag_to_imu_eulers = {
+    ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_PHI),
+    ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_THETA),
+    ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_PSI)
+  };
+  int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
+#endif
 }
 
 void mag_hmc58xx_module_periodic(void)
@@ -81,8 +98,17 @@ void mag_hmc58xx_module_event(void)
       HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
       HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
     };
+    // only rotate if needed
+#if USE_MAG_TO_IMU
+    struct Int32Vect3 imu_mag;
+    // rotate data from mag frame to imu frame
+    int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
+    // unscaled vector
+    VECT3_COPY(imu.mag_unscaled, imu_mag);
+#else
     // unscaled vector
     VECT3_COPY(imu.mag_unscaled, mag);
+#endif
     // scale vector
     imu_scale_mag(&imu);