diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index d812d41f76..2b494765b2 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -269,10 +269,8 @@ AP_HAL::AnalogSource * batt_curr_pin;
 ////////////////////////////////////////////////////////////////////////////////
 //
 ModeFilterInt16_Size5 sonar_mode_filter(2);
-#if CONFIG_SONAR == ENABLED
-    AP_HAL::AnalogSource *sonar_analog_source;
-    AP_RangeFinder_MaxsonarXL *sonar;
-#endif
+AP_HAL::AnalogSource *sonar_analog_source;
+AP_RangeFinder_MaxsonarXL *sonar;
 
 // relay support
 AP_Relay relay;
@@ -582,19 +580,15 @@ void setup() {
     batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
     batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
 
-#if CONFIG_SONAR == ENABLED
  #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
     sonar_analog_source = new AP_ADC_AnalogSource(
         &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
  #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
     sonar_analog_source = hal.analogin->channel(
         CONFIG_SONAR_SOURCE_ANALOG_PIN);
- #else
-  #warning "Invalid CONFIG_SONAR_SOURCE"
- #endif
     sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,
                                           &sonar_mode_filter);
-#endif
+ #endif
 
 	init_ardupilot();
 }
@@ -676,7 +670,6 @@ static void fast_loop()
 #endif 
 	// Read Sonar
 	// ----------
-#if CONFIG_SONAR == ENABLED
 	if(g.sonar_enabled){
 		sonar_dist = sonar->read();
 
@@ -686,7 +679,6 @@ static void fast_loop()
             obstacle = false;
             }
 	}
-#endif
 
 	// uses the yaw from the DCM to give more accurate turns
 	calc_bearing_error();
diff --git a/APMrover2/config.h b/APMrover2/config.h
index 795c838c5e..7d70fc21c7 100644
--- a/APMrover2/config.h
+++ b/APMrover2/config.h
@@ -76,6 +76,7 @@
 # define CONFIG_RELAY     ENABLED
 # define BATTERY_PIN_1	  0
 # define CURRENT_PIN_1	  1
+# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ADC
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
 # define CONFIG_INS_TYPE   CONFIG_INS_MPU6000
 # define CONFIG_PUSHBUTTON DISABLED
@@ -129,10 +130,6 @@
 # define MAG_ORIENTATION   ROTATION_NONE
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-#define CONFIG_SONAR DISABLED
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // IMU Selection
 //
@@ -184,14 +181,6 @@
 # define SONAR_ENABLED DISABLED
 #endif
 
-#ifndef CONFIG_SONAR
-# define CONFIG_SONAR ENABLED
-#endif
-
-#ifndef SONAR_TRIGGER
-# define SONAR_TRIGGER       60        // trigger distance in cm
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // HIL_MODE                                 OPTIONAL
 
diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde
index 1f697ccfeb..cdcc052763 100644
--- a/APMrover2/sensors.pde
+++ b/APMrover2/sensors.pde
@@ -1,5 +1,4 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-#if CONFIG_SONAR == ENABLED
 static void init_sonar(void)
 {
   /*
@@ -10,7 +9,6 @@ static void init_sonar(void)
 	#endif
 */
 }
-#endif
 
 #if LITE == DISABLED
 // Sensors are not available in HIL_MODE_ATTITUDE
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index 74917fe34d..9c6afd0b98 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -221,10 +221,7 @@ static void init_ardupilot()
 
 #endif
 	// initialise sonar
-	#if CONFIG_SONAR == ENABLED
 	init_sonar();
-	#endif
-
 #endif
 	// Do GPS init
 	g_gps = &g_gps_driver;
diff --git a/APMrover2/test.pde b/APMrover2/test.pde
index c161c949c3..d5f51d76af 100644
--- a/APMrover2/test.pde
+++ b/APMrover2/test.pde
@@ -16,9 +16,7 @@ static int8_t	test_ins(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_battery(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_relay(uint8_t argc,	 	const Menu::arg *argv);
 static int8_t	test_wp(uint8_t argc, 			const Menu::arg *argv);
-#if CONFIG_SONAR == ENABLED
 static int8_t	test_sonar(uint8_t argc, 	const Menu::arg *argv);
-#endif
 static int8_t	test_mag(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_modeswitch(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_logging(uint8_t argc, 		const Menu::arg *argv);
@@ -45,9 +43,7 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
 #endif
 	{"gps",			test_gps},
 	{"ins",			test_ins},
-#if CONFIG_SONAR == ENABLED
 	{"sonartest",	test_sonar},
-#endif
 	{"compass",		test_mag},
 #elif HIL_MODE == HIL_MODE_SENSORS
 	{"adc", 		test_adc},
@@ -546,28 +542,28 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 //-------------------------------------------------------------------------------------------
 // real sensors that have not been simulated yet go here
 
-#if CONFIG_SONAR == ENABLED
 static int8_t
 test_sonar(uint8_t argc, const Menu::arg *argv)
 {
-  print_hit_enter();
+    if (!g.sonar_enabled) {
+        cliSerial->println_P(PSTR("Sonar is not enabled"));
+        return 0;
+    }
+
+    print_hit_enter();
 	delay(1000);
 	init_sonar();
 	delay(1000);
 
-	while(1){
-	  delay(20);
-	  if(g.sonar_enabled){
-		sonar_dist = sonar->read();
-	  }
-    	  cliSerial->printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
-
-          if(cliSerial->available() > 0){
-  		break;
+	while (true) {
+        delay(20);
+        sonar_dist = sonar->read();
+        cliSerial->printf_P(PSTR("sonar distance = %d cm\n"), (int)sonar_dist);
+        if (cliSerial->available() > 0) {
+            break;
 	    }
-  }
-  return (0);
+    }
+    return (0);
 }
-#endif // SONAR == ENABLED
 
 #endif // CLI_ENABLED