diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml
index 22aea933d4..f60a232485 100644
--- a/conf/airframes/ardrone2_raw.xml
+++ b/conf/airframes/ardrone2_raw.xml
@@ -5,6 +5,7 @@
   <firmware name="rotorcraft">
     <target name="ap" board="ardrone2_raw">
       <define name="USE_INS_NAV_INIT"/>
+      <define name="USE_SONAR"/>
       <configure name="AHRS_PROPAGATE_FREQUENCY" value="200"/>
       <define name="AHRS_PROPAGATE_FREQUENCY" value="200"/>
       <define name="INS_PROPAGATE_FREQUENCY" value="200"/>
@@ -107,6 +108,10 @@
   </section>
 
   <section name="INS" prefix="INS_">
+    <define name="SONAR_SENS" value="0.00034"/>
+    <define name="SONAR_MAX_RANGE" value="2.2"/>
+    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
+    <define name="SONAR_VARIANCE_THRESHOLD" VALUE="2.0"/>
   </section>
 
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c
index 75f115d2c8..123045b632 100644
--- a/sw/airborne/boards/ardrone/navdata.c
+++ b/sw/airborne/boards/ardrone/navdata.c
@@ -49,7 +49,6 @@ static void navdata_cropbuffer(int cropsize);
 
 navdata_port nav_port;
 static int nav_fd = 0;
-static int16_t previousUltrasoundHeight;
 measures_t navdata;
 
 #include "subsystems/sonar.h"
@@ -190,7 +189,6 @@ bool_t navdata_init()
   navdata_imu_available = FALSE;
   navdata_baro_available = FALSE;
 
-  previousUltrasoundHeight = 0;
   nav_port.checksum_errors = 0;
   nav_port.lost_imu_frames = 0;
   nav_port.bytesRead = 0;
@@ -456,15 +454,14 @@ void navdata_update()
 
 
 #ifdef USE_SONAR
-        if (navdata.ultrasound < 10000)
+        // Check if there is a new sonar measurement and update the sonar
+        if (navdata.ultrasound >> 15)
         {
-            sonar_meas = navdata.ultrasound;
+            sonar_meas = (navdata.ultrasound & 0x7FFF);
             ins_update_sonar();
-
         }
 #endif
 
-
         navdata_imu_available = TRUE;
         last_checksum_wrong = FALSE;
         nav_port.packetsRead++;
@@ -487,17 +484,7 @@ void navdata_update()
       }
     }
   }
-}
 
-int16_t navdata_height(void) {
-  if (navdata.ultrasound > 10000) {
-    return previousUltrasoundHeight;
-  }
-
-  int16_t ultrasoundHeight = 0;
-  ultrasoundHeight = (navdata.ultrasound - 880) / 26.553;
-  previousUltrasoundHeight = ultrasoundHeight;
-  return ultrasoundHeight;
 }
 
 static void navdata_cropbuffer(int cropsize)
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index c70f43b4aa..cbbdfb67b1 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -58,6 +58,7 @@
 
 
 #if USE_SONAR
+ #include "subsystems/sonar.h"
 
 #if !USE_VFF_EXTENDED
 #error USE_SONAR needs USE_VFF_EXTENDED
@@ -80,6 +81,11 @@
 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
 #endif
 
+#ifndef INS_SONAR_UPDATE_ON_AGL
+#define INS_SONAR_UPDATE_ON_AGL FALSE
+PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
+#endif
+
 
 /** default barometer to use in INS */
 #ifndef INS_BARO_ID
@@ -137,7 +143,7 @@ void ins_init(void) {
   ins_impl.baro_initialized = FALSE;
 
 #if USE_SONAR
-  ins_impl.update_on_agl = FALSE;
+  ins_impl.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
   init_median_filter(&ins_impl.sonar_median);
   ins_impl.sonar_offset = INS_SONAR_OFFSET;
 #endif