diff --git a/conf/settings/estimation/imu_to_body.xml b/conf/settings/estimation/imu_to_body.xml
index 9fb0cb73f9..9ff8492f45 100644
--- a/conf/settings/estimation/imu_to_body.xml
+++ b/conf/settings/estimation/imu_to_body.xml
@@ -6,6 +6,8 @@
       <dl_setting MAX="90" MIN="-90" STEP="0.5" VAR="imu.body_to_imu.eulers_f.phi" shortname="imu2body phi" module="subsystems/imu" param="IMU_BODY_TO_IMU_PHI" unit="rad" alt_unit="deg" handler="SetBodyToImuPhi"/>
       <dl_setting MAX="90" MIN="-90" STEP="0.5" VAR="imu.body_to_imu.eulers_f.theta" shortname="imu2body theta" module="subsystems/imu" param="IMU_BODY_TO_IMU_THETA" unit="rad" alt_unit="deg" handler="SetBodyToImuTheta"/>
       <dl_setting MAX="180" MIN="-180" STEP="0.5" VAR="imu.body_to_imu.eulers_f.psi" shortname="imu2body psi" module="subsystems/imu" param="IMU_BODY_TO_IMU_PSI" unit="rad" alt_unit="deg" handler="SetBodyToImuPsi"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="imu.b2i_set_current" shortname="imu2body cur roll/pitch" module="subsystems/imu" handler="SetBodyToImuCurrent"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="imu.b2i_reset" shortname="imu2body reset" module="subsystems/imu" handler="ResetBodyToImu"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h
index 8ef8826384..21ce48961a 100644
--- a/sw/airborne/subsystems/imu.h
+++ b/sw/airborne/subsystems/imu.h
@@ -30,6 +30,7 @@
 #include "math/pprz_algebra_int.h"
 #include "math/pprz_algebra_float.h"
 #include "math/pprz_orientation_conversion.h"
+#include "state.h"
 #include "generated/airframe.h"
 
 /* must be defined by underlying hardware */
@@ -50,6 +51,8 @@ struct Imu {
   struct Int32Vect3 accel_unscaled;   ///< unscaled accelerometer measurements
   struct Int32Vect3 mag_unscaled;     ///< unscaled magnetometer measurements
   struct OrientationReps body_to_imu; ///< rotation from body to imu frame
+  bool_t b2i_set_current;             ///< flag for settings in order to set current roll and pitch as body_to_imu
+  bool_t b2i_reset;                   ///< flag for settings in order to reset body_to_imu to airframe defines
 };
 
 /** abstract IMU interface providing floating point interface  */
@@ -164,5 +167,26 @@ static inline void imu_SetBodyToImuPsi(float psi) {
   orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
 }
 
+static inline void imu_SetBodyToImuCurrent(float set) {
+  imu.b2i_set_current = set;
+  if (imu.b2i_set_current) {
+    struct FloatEulers imu_to_body_eulers;
+    memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
+    // set to current roll and pitch
+    imu_to_body_eulers.phi = stateGetNedToBodyEulers_f()->phi;
+    imu_to_body_eulers.theta = stateGetNedToBodyEulers_f()->theta;
+    orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
+  }
+}
+
+static inline void imu_ResetBodyToImu(float reset) {
+  imu.b2i_reset = reset;
+  if (imu.b2i_reset) {
+    struct FloatEulers imu_to_body_eulers =
+      {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
+    orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
+  }
+}
+
 
 #endif /* IMU_H */