Cleanup sonar support for Naze/Flip32/Olimexino. Add Sonar

documentation.

Makefile

@@ -220,6 +220,7 @@ OLIMEXINO_SRC	 = startup_stm32f10x_md_gcc.S \
 		   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/Sonar.md

@@ -0,0 +1,29 @@
+# Sonar
+
+A sonar sensor can be used to measure altitude for use with altitude hold modes.
+
+The current sensor takes over from the pressure sensor at low altitudes, but only when
+the angle of the aircraft is small.
+ 
+## Hardware
+
+Currently the only supported sensor is the HCSR04 sensor.
+
+## Connections
+
+### Naze
+
+| Mode          | Trigger       | Echo          | Inline 1k resistors |
+| ------------- | ------------- | ------------- | ------------------- |
+| Parallel PWM  | PB8 / Motor 5 | PB9 / Motor 6 | NO (5v tolerant)    |
+| PPM/Serial RX | PB0 / RC7     | PB1 / RC8     | YES (3.3v input)    |
+
+Current meter cannot be used in conjunction with Parallel PWM and Sonar.
+
+### Olimexino
+
+| Trigger       | Echo          | Inline 1k resistors |
+| ------------- | ------------- | ------------------- |
+| PB0 / RC7     | PB1 / RC8     | YES (3.3v input)    |
+
+Current meter cannot be used in conjunction sonar.

src/main/config/config.c

@@ -458,7 +458,7 @@ void validateAndFixConfig(void)
         featureClear(FEATURE_RX_PPM);
     }
 
-    if (feature(FEATURE_RX_PARALLEL_PWM)) {
+    if (feature(FEATURE_CURRENT_METER)) {
 #if defined(STM32F10X_MD)
         // rssi adc needs the same ports
         featureClear(FEATURE_RSSI_ADC);
@@ -471,7 +471,6 @@ void validateAndFixConfig(void)
         featureClear(FEATURE_LED_STRIP);
 #endif
 
-
         // software serial needs free PWM ports
         featureClear(FEATURE_SOFTSERIAL);
     }
@@ -484,6 +483,17 @@ void validateAndFixConfig(void)
     }
 #endif
 
+#if defined(NAZE) && defined(SONAR)
+    if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) {
+        featureClear(FEATURE_CURRENT_METER);
+    }
+#endif
+
+#if defined(OLIMEXINO) && defined(SONAR)
+    if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) {
+        featureClear(FEATURE_CURRENT_METER);
+    }
+#endif
 
     useRxConfig(&masterConfig.rxConfig);
 

src/main/drivers/sonar_hcsr04.c

@@ -36,20 +36,18 @@
  *
  */
 
-static uint16_t trigger_pin;
-static uint16_t echo_pin;
-static uint32_t exti_line;
-static uint8_t exti_pin_source;
-static IRQn_Type exti_irqn;
-
 static uint32_t last_measurement;
 static volatile int32_t *distance_ptr = 0;
 
+extern int16_t debug[4];
+
+static sonarHardware_t const *sonarHardware;
+
 void ECHO_EXTI_IRQHandler(void)
 {
     static uint32_t timing_start;
     uint32_t timing_stop;
-    if (digitalIn(GPIOB, echo_pin) != 0) {
+    if (digitalIn(GPIOB, sonarHardware->echo_pin) != 0) {
         timing_start = micros();
     } else {
         timing_stop = micros();
@@ -70,7 +68,7 @@ void ECHO_EXTI_IRQHandler(void)
         }
     }
 
-    EXTI_ClearITPendingBit(exti_line);
+    EXTI_ClearITPendingBit(sonarHardware->exti_line);
 }
 
 void EXTI1_IRQHandler(void)
@@ -83,54 +81,39 @@ void EXTI9_5_IRQHandler(void)
     ECHO_EXTI_IRQHandler();
 }
 
-void hcsr04_init(sonar_config_t config)
+void hcsr04_init(const sonarHardware_t *initialSonarHardware)
 {
     gpio_config_t gpio;
     EXTI_InitTypeDef EXTIInit;
 
+    sonarHardware = initialSonarHardware;
+
     // enable AFIO for EXTI support
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
 
-    switch (config) {
-        case sonar_pwm56:
-            trigger_pin = Pin_8;   // PWM5 (PB8) - 5v tolerant
-            echo_pin = Pin_9;      // PWM6 (PB9) - 5v tolerant
-            exti_line = EXTI_Line9;
-            exti_pin_source = GPIO_PinSource9;
-            exti_irqn = EXTI9_5_IRQn;
-            break;
-        case sonar_rc78:
-            trigger_pin = Pin_0;   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
-            echo_pin = Pin_1;      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
-            exti_line = EXTI_Line1;
-            exti_pin_source = GPIO_PinSource1;
-            exti_irqn = EXTI1_IRQn;
-            break;
-    }
-
     // tp - trigger pin
-    gpio.pin = trigger_pin;
+    gpio.pin = sonarHardware->trigger_pin;
     gpio.mode = Mode_Out_PP;
     gpio.speed = Speed_2MHz;
     gpioInit(GPIOB, &gpio);
 
     // ep - echo pin
-    gpio.pin = echo_pin;
+    gpio.pin = sonarHardware->echo_pin;
     gpio.mode = Mode_IN_FLOATING;
     gpioInit(GPIOB, &gpio);
 
     // setup external interrupt on echo pin
-    gpioExtiLineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
+    gpioExtiLineConfig(GPIO_PortSourceGPIOB, sonarHardware->exti_pin_source);
 
-    EXTI_ClearITPendingBit(exti_line);
+    EXTI_ClearITPendingBit(sonarHardware->exti_line);
 
-    EXTIInit.EXTI_Line = exti_line;
+    EXTIInit.EXTI_Line = sonarHardware->exti_line;
     EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
     EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
     EXTIInit.EXTI_LineCmd = ENABLE;
     EXTI_Init(&EXTIInit);
 
-    NVIC_EnableIRQ(exti_irqn);
+    NVIC_EnableIRQ(sonarHardware->exti_irqn);
 
     last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance()
 }
@@ -149,9 +132,13 @@ void hcsr04_get_distance(volatile int32_t *distance)
     last_measurement = current_time;
     distance_ptr = distance;
 
-    digitalHi(GPIOB, trigger_pin);
+#if 1
+    debug[0] = *distance;
+#endif
+
+    digitalHi(GPIOB, sonarHardware->trigger_pin);
     //  The width of trig signal must be greater than 10us
     delayMicroseconds(11);
-    digitalLo(GPIOB, trigger_pin);
+    digitalLo(GPIOB, sonarHardware->trigger_pin);
 }
 #endif

src/main/drivers/sonar_hcsr04.h

@@ -17,10 +17,14 @@
 
 #pragma once
 
-typedef enum {
+typedef struct sonarHardware_s {
-    sonar_pwm56,
+    uint16_t trigger_pin;
-    sonar_rc78,
+    uint16_t echo_pin;
-} sonar_config_t;
+    uint32_t exti_line;
+    uint8_t exti_pin_source;
+    IRQn_Type exti_irqn;
+} sonarHardware_t;
+
+void hcsr04_init(const sonarHardware_t *sonarHardware);
 
-void hcsr04_init(sonar_config_t config);
 void hcsr04_get_distance(volatile int32_t *distance);

src/main/mw.c

@@ -612,8 +612,8 @@ void loop(void)
             if (f.BARO_MODE) {
                 updateAltHold();
             }
-            debug[0] = rcCommand[THROTTLE];
         }
+
 #endif
 
         if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {

src/main/sensors/sonar.c

@@ -24,6 +24,7 @@
 #include "common/axis.h"
 
 #include "drivers/sonar_hcsr04.h"
+#include "drivers/gpio.h"
 #include "config/runtime_config.h"
 #include "config/config.h"
 
@@ -38,12 +39,39 @@ int32_t sonarAlt = -1;	// in cm , -1 indicate sonar is not in range
 
 void Sonar_init(void)
 {
-	// 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 defined(NAZE)
-	if (feature(FEATURE_RX_PARALLEL_PWM)) {
+    static const sonarHardware_t const sonarPWM56 = {
-		hcsr04_init(sonar_pwm56);
+        .trigger_pin = Pin_8,   // PWM5 (PB8) - 5v tolerant
-	} else {
+        .echo_pin = Pin_9,      // PWM6 (PB9) - 5v tolerant
-		hcsr04_init(sonar_rc78);
+        .exti_line = EXTI_Line9,
-	}
+        .exti_pin_source = GPIO_PinSource9,
+        .exti_irqn = EXTI9_5_IRQn
+    };
+    static const sonarHardware_t const sonarRC78 = {
+        .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
+        .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
+        .exti_line = EXTI_Line1,
+        .exti_pin_source = GPIO_PinSource1,
+        .exti_irqn = EXTI1_IRQn
+    };
+    // 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(&sonarPWM56);
+    } else {
+        hcsr04_init(&sonarRC78);
+    }
+#elif defined(OLIMEXINO)
+    static const sonarHardware_t const sonarHardware = {
+        .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
+        .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
+        .exti_line = EXTI_Line1,
+        .exti_pin_source = GPIO_PinSource1,
+        .exti_irqn = EXTI1_IRQn
+    };
+    hcsr04_init(&sonarHardware);
+#else
+#error Sonar not defined for target
+#endif
 
     sensorsSet(SENSOR_SONAR);
     sonarAlt = 0;

src/main/target/OLIMEXINO/target.h

@@ -39,6 +39,7 @@
 #define BARO
 #define GYRO
 #define MAG
+#define SONAR
 
 #define USE_USART1
 #define USE_USART2