CC3D - Add sonar.

This also ensures that the PWM mapping does not use the sonar pins when
sonar is enabled in a board agnostic way.
Conflicts:
	src/main/config/config.c
	src/main/drivers/pwm_mapping.h
	src/main/main.c
	src/main/target/CC3D/target.h

Makefile

@@ -444,6 +444,7 @@ CC3D_SRC	 = \
 		   drivers/serial_softserial.c \
 		   drivers/serial_uart.c \
 		   drivers/serial_uart_stm32f10x.c \
+		   drivers/sonar_hcsr04.c \
 		   drivers/sound_beeper_stm32f10x.c \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \

docs/Board - CC3D.md

@@ -23,8 +23,8 @@ The 8 pin RC_Input connector has the following pinouts when used in RX_PPM/RX_SE
 | 1   | Ground    |                                  |
 | 2   | +5V       |                                  |
 | 3   | PPM Input | Enable `feature RX_PPM`          | 
-| 4   | SoftSerial1 TX | Enable `feature SOFTSERIAL` |
+| 4   | SoftSerial1 TX / Sonar trigger | |
-| 5   | SoftSerial1 RX | Enable `feature SOFTSERIAL` |
+| 5   | SoftSerial1 RX / Sonar Echo    | |
 | 6   | Current   | Enable `feature CURRENT_METER`.  Connect to the output of a current sensor, 0v-3.3v input |
 | 7   | Battery Voltage sensor | Enable `feature VBAT`. Connect to main battery using a voltage divider, 0v-3.3v input |
 | 8   | RSSI      | Enable `feature RSSI_ADC`.  Connect to the output of a PWM-RSSI conditioner, 0v-3.3v input |

docs/Sonar.md

@@ -19,6 +19,8 @@ Currently the only supported sensor is the HCSR04 sensor.
 | Parallel PWM/ADC current sensor | PB8 / Motor 5 | PB9 / Motor 6 | NO (5v tolerant)    |
 | PPM/Serial RX                   | PB0 / RC7     | PB1 / RC8     | YES (3.3v input)    |
 
+#### Constraints
+
 Current meter cannot be used in conjunction with Parallel PWM and Sonar.
 
 ### Olimexino
@@ -27,4 +29,16 @@ Current meter cannot be used in conjunction with Parallel PWM and Sonar.
 | ------------- | ------------- | ------------------- |
 | PB0 / RC7     | PB1 / RC8     | YES (3.3v input)    |
 
-Current meter cannot be used in conjunction with sonar.
+#### Constraints
+
+Current meter cannot be used in conjunction with Sonar.
+
+### CC3D
+
+| Trigger       | Echo          | Inline 1k resistors |
+| ------------- | ------------- | ------------------- |
+| PB5           | PB0           | YES (3.3v input)    |
+
+#### Constraints
+
+Sonar cannot be used in conjuction with SoftSerial or Parallel PWM.

src/main/config/config.c

@@ -804,6 +804,12 @@ void validateAndFixConfig(void)
         masterConfig.mixerConfig.pid_at_min_throttle = 0;
     }
 
+#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1)
+    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
+        featureClear(FEATURE_SONAR);
+    }
+#endif
+
     useRxConfig(&masterConfig.rxConfig);
 
     serialConfig_t *serialConfig = &masterConfig.serialConfig;

src/main/drivers/pwm_mapping.c

@@ -408,29 +408,6 @@ 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;
@@ -471,6 +448,17 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
         }
 #endif
 
+#ifdef SONAR
+        if (init->sonarGPIOConfig && timerHardwarePtr->gpio == init->sonarGPIOConfig->gpio &&
+            (
+                timerHardwarePtr->pin == init->sonarGPIOConfig->triggerPin ||
+                timerHardwarePtr->pin == init->sonarGPIOConfig->echoPin
+            )
+        ) {
+            continue;
+        }
+#endif
+
         // hacks to allow current functionality
         if (type == MAP_TO_PWM_INPUT && !init->useParallelPWM)
             continue;

src/main/drivers/pwm_mapping.h

@@ -36,6 +36,12 @@
 #define PWM_TIMER_MHZ 1
 #define ONESHOT125_TIMER_MHZ 8
 
+typedef struct sonarGPIOConfig_s {
+    GPIO_TypeDef *gpio;
+    uint16_t triggerPin;
+    uint16_t echoPin;
+} sonarGPIOConfig_t;
+
 typedef struct drv_pwm_config_t {
     bool useParallelPWM;
     bool usePPM;
@@ -65,6 +71,7 @@ typedef struct drv_pwm_config_t {
     uint16_t motorPwmRate;
     uint16_t idlePulse;  // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
                          // some higher value (used by 3d mode), or 0, for brushed pwm drivers.
+    sonarGPIOConfig_t *sonarGPIOConfig;
 } drv_pwm_config_t;
 
 

src/main/drivers/sonar_hcsr04.c

@@ -58,6 +58,11 @@ static void ECHO_EXTI_IRQHandler(void)
     EXTI_ClearITPendingBit(sonarHardware->exti_line);
 }
 
+void EXTI0_IRQHandler(void)
+{
+    ECHO_EXTI_IRQHandler();
+}
+
 void EXTI1_IRQHandler(void)
 {
     ECHO_EXTI_IRQHandler();

src/main/drivers/sonar_hcsr04.h

@@ -25,6 +25,8 @@ typedef struct sonarHardware_s {
     IRQn_Type exti_irqn;
 } sonarHardware_t;
 
+#define SONAR_GPIO GPIOB
+
 void hcsr04_init(const sonarHardware_t *sonarHardware);
 
 void hcsr04_start_reading(void);

src/main/drivers/timer.h

@@ -60,7 +60,7 @@ typedef struct timerOvrHandlerRec_s {
 typedef struct {
     TIM_TypeDef *tim;
     GPIO_TypeDef *gpio;
-    uint32_t pin;
+    uint16_t pin;
     uint8_t channel;
     uint8_t irq;
     uint8_t outputEnable;

src/main/main.c

@@ -47,6 +47,7 @@
 #include "drivers/bus_spi.h"
 #include "drivers/inverter.h"
 #include "drivers/flash_m25p16.h"
+#include "drivers/sonar_hcsr04.h"
 
 #include "rx/rx.h"
 
@@ -114,6 +115,8 @@ void displayInit(rxConfig_t *intialRxConfig);
 void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
 void loop(void);
 void spektrumBind(rxConfig_t *rxConfig);
+const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
+void sonarInit(const sonarHardware_t *sonarHardware);
 
 #ifdef STM32F303xC
 // from system_stm32f30x.c
@@ -193,6 +196,21 @@ void init(void)
     mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
 
     memset(&pwm_params, 0, sizeof(pwm_params));
+
+#ifdef SONAR
+    const sonarHardware_t *sonarHardware = NULL;
+
+    if (feature(FEATURE_SONAR)) {
+        sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
+        sonarGPIOConfig_t sonarGPIOConfig = {
+                .echoPin = sonarHardware->trigger_pin,
+                .triggerPin = sonarHardware->echo_pin,
+                .gpio = SONAR_GPIO
+        };
+        pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
+    }
+#endif
+
     // when using airplane/wing mixer, servo/motor outputs are remapped
     if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
         pwm_params.airplane = true;
@@ -383,7 +401,7 @@ void init(void)
 
 #ifdef SONAR
     if (feature(FEATURE_SONAR)) {
-        sonarInit(&masterConfig.batteryConfig);
+        sonarInit(sonarHardware);
     }
 #endif
 

src/main/sensors/sonar.c

@@ -38,7 +38,7 @@
 
 static int32_t calculatedAltitude;
 
-void sonarInit(batteryConfig_t *batteryConfig)
+const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig) 
 {
 #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
     static const sonarHardware_t const sonarPWM56 = {
@@ -57,9 +57,9 @@ void sonarInit(batteryConfig_t *batteryConfig)
     };
     // If we are using parallel PWM for our receiver or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
     if (feature(FEATURE_RX_PARALLEL_PWM ) || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC) ) {
-        hcsr04_init(&sonarPWM56);
+        return &sonarPWM56;
     } else {
-        hcsr04_init(&sonarRC78);
+        return &sonarRC78;
     }
 #elif defined(OLIMEXINO)
     UNUSED(batteryConfig);
@@ -70,7 +70,17 @@ void sonarInit(batteryConfig_t *batteryConfig)
         .exti_pin_source = GPIO_PinSource1,
         .exti_irqn = EXTI1_IRQn
     };
-    hcsr04_init(&sonarHardware);
+    return &sonarHardware;
+#elif defined(CC3D)
+    UNUSED(batteryConfig);
+    static const sonarHardware_t const sonarHardware = {
+        .trigger_pin = Pin_5,   // (PB5)
+        .echo_pin = Pin_0,      // (PB0) - only 3.3v ( add a 1K Ohms resistor )
+        .exti_line = EXTI_Line0,
+        .exti_pin_source = GPIO_PinSource0,
+        .exti_irqn = EXTI0_IRQn
+    };
+    return &sonarHardware;
 #elif defined(SPRACINGF3)
     UNUSED(batteryConfig);
     static const sonarHardware_t const sonarHardware = {
@@ -80,11 +90,15 @@ void sonarInit(batteryConfig_t *batteryConfig)
         .exti_pin_source = EXTI_PinSource1,
         .exti_irqn = EXTI1_IRQn
     };
-    hcsr04_init(&sonarHardware);
+    return &sonarHardware;
 #else
 #error Sonar not defined for target
 #endif
+}
 
+void sonarInit(const sonarHardware_t *sonarHardware)
+{
+    hcsr04_init(sonarHardware);
     sensorsSet(SENSOR_SONAR);
     calculatedAltitude = -1;
 }

src/main/sensors/sonar.h

@@ -18,9 +18,9 @@
 #pragma once
 #include "sensors/battery.h"
 
-void sonarInit(batteryConfig_t *batteryConfig);
 void sonarUpdate(void);
 
 int32_t sonarRead(void);
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
 int32_t sonarGetLatestAltitude(void);
+

src/main/target/CC3D/target.h

@@ -111,6 +111,7 @@
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
+#define SONAR
 #define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
@@ -119,6 +120,7 @@
 #undef AUTOTUNE // disabled for OPBL build due to code size.
 #endif
 
+
 #define SPEKTRUM_BIND
 // USART3, PB11 (Flexport)
 #define BIND_PORT  GPIOB