From b04be157b12329d30c41b1bf187b254e81a31c8c Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Wed, 6 May 2015 21:28:25 +0100
Subject: [PATCH] SPRacingF3 - Allow Sonar and Parallel PWM (drops 2 input
 channels). Naze32 - Fix initialisation of outputs 5/6 when Parallel PWM and
 Sonar are used together with a mixer that needs more than 4 motors.

---
 src/main/config/config.c       |  6 ------
 src/main/drivers/pwm_mapping.c | 23 +++++++++++++++++++++++
 src/main/drivers/pwm_mapping.h |  3 +++
 src/main/main.c                |  3 +++
 4 files changed, 29 insertions(+), 6 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 05e1c55ec2..145ae77409 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -790,12 +790,6 @@ void validateAndFixConfig(void)
     }
 #endif
 
-#if defined(SPRACINGF3) && defined(SONAR)
-    if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) ) {
-        featureClear(FEATURE_SONAR);
-    }
-#endif
-
     /*
      * The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
      * The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index b6bb74b789..88a94a7763 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -381,6 +381,29 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
             continue;
 #endif
 
+#ifdef SONAR
+        // skip Sonar pins
+        // FIXME - Hack - See sonar.c sonarInit() and sonarHardware_t
+        if (init->useSonar && timerHardwarePtr->gpio == GPIOB) {
+#if defined(SPRACINGF3) || defined(OLIMEXINO)
+            if (timerHardwarePtr->pin == GPIO_Pin_0 || timerHardwarePtr->pin == GPIO_Pin_1) {
+                continue;
+            }
+#endif
+#if defined(NAZE)
+            if (init->useParallelPWM) {
+                if (timerHardwarePtr->pin == GPIO_Pin_8 || timerHardwarePtr->pin == GPIO_Pin_9) {
+                    continue;
+                }
+            } else {
+                if (timerHardwarePtr->pin == GPIO_Pin_0 || timerHardwarePtr->pin == GPIO_Pin_1) {
+                    continue;
+                }
+            }
+#endif
+        }
+#endif
+
 #ifdef SOFTSERIAL_1_TIMER
         if (init->useSoftSerial && timerHardwarePtr->tim == SOFTSERIAL_1_TIMER)
             continue;
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index 246246a7ef..6f06397f4d 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -52,6 +52,9 @@ typedef struct drv_pwm_config_t {
     bool useOneshot;
     bool useSoftSerial;
     bool useLEDStrip;
+#ifdef SONAR
+    bool useSonar;
+#endif
 #ifdef USE_SERVOS
     bool useServos;
     bool extraServos;    // configure additional 4 channels in PPM mode as servos, not motors
diff --git a/src/main/main.c b/src/main/main.c
index 19da73c1c5..227ac2811d 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -213,6 +213,9 @@ void init(void)
     pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
     pwm_params.usePPM = feature(FEATURE_RX_PPM);
     pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
+#ifdef SONAR
+    pwm_params.useSonar = feature(FEATURE_SONAR);
+#endif
 
 #ifdef USE_SERVOS
     pwm_params.useServos = isMixerUsingServos();