adding sonar support from sbaron, disabled by default.

docs: http://www.rcgroups.com/forums/showpost.php?p=21903320&postcount=3827

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@166 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

src/board.h

@@ -45,7 +45,8 @@ typedef enum {
     FEATURE_GYRO_SMOOTHING = 1 << 7,
     FEATURE_LED_RING = 1 << 8,
     FEATURE_GPS = 1 << 9,
-    FEATURE_FAILSAFE = 1 << 10
+    FEATURE_FAILSAFE = 1 << 10,
+    FEATURE_SONAR = 1 << 11,
 } AvailableFeatures;
 
 typedef void (* sensorInitFuncPtr)(void);                   // sensor init prototype
@@ -92,6 +93,7 @@ typedef struct sensor_t
 #define MAG
 #define BARO
 #define LEDRING
+// #define SONAR
 
 #endif
 
@@ -136,5 +138,6 @@ typedef struct sensor_t
 #include "drv_mpu6050.h"
 #include "drv_pwm.h"
 #include "drv_uart.h"
+#include "drv_hcsr04.h"
 
 #endif

src/cli.c

@@ -37,7 +37,7 @@ const char *mixerNames[] = {
 const char *featureNames[] = {
     "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
     "SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING", "GPS",
-    "FAILSAFE",
+    "FAILSAFE", "SONAR",
     NULL
 };
 

src/drv_hcsr04.c

@@ -0,0 +1,132 @@
+#include "board.h"
+#include "mw.h"
+
+#ifdef SONAR
+
+/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
+ * When trigged it sends out a series of 40KHz ultrasonic pulses and receives
+ * echo froman object. The distance between the unit and the object is calculated
+ * by measuring the traveling time of sound and output it as the width of a TTL pulse.
+ *
+ * *** Warning: HC-SR04 operates at +5V ***
+ *
+ */
+
+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 int16_t* distance_ptr;
+
+void ECHO_EXTI_IRQHandler(void)
+{
+    static uint32_t timing_start;
+    uint32_t timing_stop;
+    if(GPIO_ReadInputDataBit(GPIOB, echo_pin) != 0)
+        timing_start = micros();
+    else 
+    {
+        timing_stop = micros();
+        if(timing_stop > timing_start) 
+        {
+            // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
+            // The ping travels out and back, so to find the distance of the
+            // object we take half of the distance traveled.
+            //
+            // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
+            int32_t pulse_duration = timing_stop - timing_start;          
+            *distance_ptr = pulse_duration / 59 ;
+        }
+    }
+
+    EXTI_ClearITPendingBit(exti_line);   
+}
+
+void EXTI1_IRQHandler(void)
+{
+    ECHO_EXTI_IRQHandler();
+}
+
+void EXTI9_5_IRQHandler(void)
+{
+    ECHO_EXTI_IRQHandler();
+}
+
+void hcsr04_init(sonar_config_t config)
+{
+    GPIO_InitTypeDef GPIO_InitStructure;
+    EXTI_InitTypeDef EXTIInit;
+
+    //enable AFIO for EXTI support - already done is drv_system.c
+    //RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE); 
+
+    switch(config)
+    {
+    case sonar_pwm56:
+        trigger_pin = GPIO_Pin_8;   // PWM5 (PB8) - 5v tolerant
+        echo_pin = GPIO_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 = GPIO_Pin_0;   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
+        echo_pin = GPIO_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_InitStructure.GPIO_Pin = trigger_pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+    GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+    // ep - echo pin
+    GPIO_InitStructure.GPIO_Pin = echo_pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+    GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+    // setup external interrupt on echo pin
+    GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
+
+    EXTI_ClearITPendingBit(exti_line);
+       
+    EXTIInit.EXTI_Line = 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);
+
+    last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance()
+}
+
+// distance calculation is done asynchronously, using interrupt
+void hcsr04_get_distance(volatile int16_t* distance)
+{   
+    uint32_t current_time = millis();
+
+    if( current_time < (last_measurement + 60) )
+    {
+        // the repeat interval of trig signal should be greater than 60ms
+        // to avoid interference between connective measurements.
+        return;
+    }
+        
+    last_measurement = current_time;
+    distance_ptr = distance;
+
+    GPIO_SetBits(GPIOB, trigger_pin);
+    //  The width of trig signal must be greater than 10us
+    delayMicroseconds(11);
+    GPIO_ResetBits(GPIOB, trigger_pin);
+}
+
+#endif

src/drv_hcsr04.h

@@ -0,0 +1,9 @@
+#pragma once
+
+typedef enum {
+    sonar_pwm56,
+    sonar_rc78,
+} sonar_config_t;
+
+void hcsr04_init(sonar_config_t config);
+void hcsr04_get_distance(volatile int16_t* distance);

src/main.c

@@ -115,8 +115,15 @@ int main(void)
     } else {
         // spektrum and GPS are mutually exclusive
         // Optional GPS - available only when using PPM, otherwise required pins won't be usable
-        if (feature(FEATURE_PPM) && feature(FEATURE_GPS))
+        if (feature(FEATURE_PPM))
+        {
+            if (feature(FEATURE_GPS))
                 gpsInit(cfg.gps_baudrate);
+#ifdef SONAR
+            if (feature(FEATURE_SONAR))
+                Sonar_init();
+#endif
+        }
     }
 
     previousTime = micros();

src/mw.c

@@ -505,8 +505,10 @@ void loop(void)
             break;
         case 3:
 #ifdef SONAR
+            if (sensors(SENSOR_SONAR)) {
                 Sonar_update();
                 debug3 = sonarAlt;
+            }
 #endif
             break;
         default:

src/mw.h

@@ -276,6 +276,8 @@ void Baro_update(void);
 void Gyro_getADC(void);
 void Mag_init(void);
 void Mag_getADC(void);
+void Sonar_init(void);
+void Sonar_update(void);
 
 // Output
 void mixerInit(void);

src/sensors.c

@@ -454,3 +454,19 @@ void Mag_getADC(void)
     }
 }
 #endif
+
+#ifdef SONAR
+
+void Sonar_init(void) 
+{
+    hcsr04_init(sonar_rc78);
+    sensorsSet(SENSOR_SONAR);
+    sonarAlt = 0;
+}
+
+void Sonar_update(void) 
+{
+    hcsr04_get_distance(&sonarAlt);
+}
+
+#endif