diff --git a/src/main/config/config.c b/src/main/config/config.c
index a33b4375..0b3062af 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -464,10 +464,6 @@ void validateAndFixConfig(void)
         featureClear(FEATURE_RSSI_ADC);
         // current meter needs the same ports
         featureClear(FEATURE_CURRENT_METER);
-#ifdef SONAR
-        // sonar needs a free PWM port
-        featureClear(FEATURE_SONAR);
-#endif
 #endif
 
 #if defined(STM32F10X_MD) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c
index 79de889a..97f8f551 100644
--- a/src/main/drivers/sonar_hcsr04.c
+++ b/src/main/drivers/sonar_hcsr04.c
@@ -88,8 +88,8 @@ void hcsr04_init(sonar_config_t config)
     gpio_config_t gpio;
     EXTI_InitTypeDef EXTIInit;
 
-    // enable AFIO for EXTI support - already done is drv_system.c
-    // RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE);
+    // enable AFIO for EXTI support
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
 
     switch (config) {
         case sonar_pwm56:
diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c
index 143c1c13..eb8039f0 100644
--- a/src/main/sensors/sonar.c
+++ b/src/main/sensors/sonar.c
@@ -25,6 +25,7 @@
 
 #include "drivers/sonar_hcsr04.h"
 #include "config/runtime_config.h"
+#include "config/config.h"
 
 #include "flight/flight.h"
 
@@ -37,7 +38,13 @@ int32_t sonarAlt = -1;	// in cm , -1 indicate sonar is not in range
 
 void Sonar_init(void)
 {
-    hcsr04_init(sonar_rc78);
+	// If we are using parallel PWM for our receiver, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
+	if (feature(FEATURE_RX_PARALLEL_PWM)) {
+		hcsr04_init(sonar_pwm56);
+	} else {
+		hcsr04_init(sonar_rc78);
+	}
+
     sensorsSet(SENSOR_SONAR);
     sonarAlt = 0;
 }