diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
index 4928c4c495..e4ecf1b674 100644
--- a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
@@ -2,7 +2,7 @@
 # extended INS with vertical filter using sonar in a better way (flap ground)
 #
 
-$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\"
+$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int_extended.h\"
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int_extended.c
 
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 3dd2985273..950397b7fa 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -73,10 +73,6 @@ struct FloatVect2 ins_gps_speed_m_s_ned;
 int32_t ins_qfe;
 bool_t  ins_baro_initialised;
 int32_t ins_baro_alt;
-#if USE_SONAR
-bool_t  ins_update_on_agl;
-int32_t ins_sonar_offset;
-#endif
 #endif
 
 /* output                      */
@@ -107,9 +103,6 @@ void ins_init() {
 #endif
 #if USE_VFF
   ins_baro_initialised = FALSE;
-#if USE_SONAR
-  ins_update_on_agl = FALSE;
-#endif
   vff_init(0., 0., 0.);
 #endif
   ins.vf_realign = FALSE;
@@ -185,9 +178,6 @@ void ins_update_baro() {
     if (ins.vf_realign) {
       ins.vf_realign = FALSE;
       ins_qfe = baro.absolute;
-#if USE_SONAR
-      ins_sonar_offset = sonar_meas;
-#endif
       vff_realign(0.);
       ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
       ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
@@ -263,13 +253,4 @@ void ins_update_gps(void) {
 }
 
 void ins_update_sonar() {
-#if USE_SONAR && USE_VFF
-  static int32_t sonar_filtered = 0;
-  sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
-  /* update baro_qfe assuming a flat ground */
-  if (ins_update_on_agl && baro.status == BS_RUNNING) {
-    int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
-    ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
-  }
-#endif
 }
diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h
index dd12b87f45..c72c805c52 100644
--- a/sw/airborne/subsystems/ins/ins_int.h
+++ b/sw/airborne/subsystems/ins/ins_int.h
@@ -49,14 +49,10 @@ extern struct NedCoor_i ins_gps_pos_cm_ned;
 extern struct NedCoor_i ins_gps_speed_cm_s_ned;
 
 /* barometer                   */
-#if USE_VFF || USE_VFF_EXTENDED
+#if USE_VFF
 extern int32_t ins_baro_alt;  ///< altitude calculated from baro in meters with #INT32_POS_FRAC
 extern int32_t ins_qfe;
 extern bool_t  ins_baro_initialised;
-#if USE_SONAR
-extern bool_t  ins_update_on_agl; /* use sonar to update agl if available */
-extern int32_t ins_sonar_offset;
-#endif
 #endif
 
 /* output LTP NED               */
diff --git a/sw/airborne/subsystems/ins/ins_int_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c
index 3215f44d70..aefa07f10f 100644
--- a/sw/airborne/subsystems/ins/ins_int_extended.c
+++ b/sw/airborne/subsystems/ins/ins_int_extended.c
@@ -27,7 +27,7 @@
  *
  */
 
-#include "subsystems/ins/ins_int.h"
+#include "subsystems/ins/ins_int_extended.h"
 
 #include "subsystems/imu.h"
 #include "subsystems/sensors/baro.h"
@@ -270,8 +270,8 @@ uint8_t var_idx = 0;
 #endif
 
 
-#if USE_SONAR
 void ins_update_sonar() {
+#if USE_SONAR
   static float last_offset = 0.;
   // new value filtered with median_filter
   ins_sonar_alt = update_median_filter(&sonar_median, sonar_meas);
@@ -318,5 +318,6 @@ void ins_update_sonar() {
     /* update offset with last value to avoid divergence */
     vff_update_offset(last_offset);
   }
-}
 #endif // USE_SONAR
+}
+