diff --git a/conf/airframes/TUDELFT/tudelft_bebop_autonomous_race.xml b/conf/airframes/TUDELFT/tudelft_bebop_autonomous_race.xml
index 7fb30c0f96..30e04b137f 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop_autonomous_race.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop_autonomous_race.xml
@@ -10,7 +10,7 @@
       <module name="udp"/>
     </target>
 
-    <!--define name="USE_SONAR" value="TRUE"/-->
+    <define name="USE_SONAR"/>
 
 	<!-- Subsystem section -->
     <module name="telemetry" type="transparent_udp"/>
@@ -35,7 +35,6 @@
     <module name="video_thread">
     </module>
 
-    <module name="sonar_bebop"/>
     <module name="pose_history"/>
 
     <module name="cv_detect_contour">
diff --git a/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml b/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
index 6d8f599797..06a16a78c3 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
@@ -10,6 +10,8 @@
       <module name="udp"/>
     </target>
 
+    <define name="USE_SONAR"/>
+
 	<!-- Subsystem section -->
     <module name="telemetry" type="transparent_udp"/>
     <module name="radio_control" type="datalink"/>
@@ -29,7 +31,6 @@
     <module name="geo_mag"/>
     <module name="air_data"/>
     <module name="send_imu_mag_current"/>
-    <module name="sonar_bebop"/> <!-- Needed for relative height est for velocity calc from opt flow-->
     <module name="pose_history"/>
     <module name="video_thread"/>
 
diff --git a/conf/modules/imu_bebop.xml b/conf/modules/imu_bebop.xml
index 4f3c268a4f..4dcde257a4 100644
--- a/conf/modules/imu_bebop.xml
+++ b/conf/modules/imu_bebop.xml
@@ -19,6 +19,7 @@
   </doc>
   <autoload name="imu_common"/>
   <autoload name="imu_nps"/>
+  <autoload name="sonar_bebop"/>
   <header>
     <file name="imu_bebop.h" dir="subsystems/imu"/>
   </header>
diff --git a/conf/modules/sonar_bebop.xml b/conf/modules/sonar_bebop.xml
index 754f9d1bbe..a363397f0a 100644
--- a/conf/modules/sonar_bebop.xml
+++ b/conf/modules/sonar_bebop.xml
@@ -21,7 +21,6 @@
   <makefile target="ap">
     <define name="USE_SPI0" value="1"/>
     <define name="USE_ADC0" value="1"/>
-    <define name="USE_SONAR" value="1"/>
 
     <raw>
       include $(CFG_SHARED)/spi_master.makefile
diff --git a/sw/airborne/modules/sonar/sonar_bebop.c b/sw/airborne/modules/sonar/sonar_bebop.c
index 3fb122498e..e72125018f 100644
--- a/sw/airborne/modules/sonar/sonar_bebop.c
+++ b/sw/airborne/modules/sonar/sonar_bebop.c
@@ -56,10 +56,9 @@ void sonar_bebop_init(void)
   sonar_bebop_spi_t.input_buf     = NULL;
   sonar_bebop_spi_t.input_length  = 0;
 
-  int rc = pthread_create(&sonar_bebop_thread, NULL, sonar_bebop_read, NULL);
-  if (rc < 0) {
-    return;
-  }
+#if USE_SONAR
+  pthread_create(&sonar_bebop_thread, NULL, sonar_bebop_read, NULL);
+#endif
 }
 
 /**