Sonar merge

src/main/drivers/sonar_hcsr04.c

@@ -19,6 +19,7 @@
 #include <stdint.h>
 
 #include "platform.h"
+#include "build_config.h"
 
 #include "system.h"
 #include "gpio.h"
@@ -26,27 +27,27 @@
 
 #include "sonar_hcsr04.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
+ * When triggered it sends out a series of 40KHz ultrasonic pulses and receives
- * echo froman object. The distance between the unit and the object is calculated
+ * echo from an 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 ***
  *
  */
 
+#if defined(SONAR)
+STATIC_UNIT_TESTED volatile int32_t measurement = -1;
 static uint32_t lastMeasurementAt;
-static volatile int32_t measurement = -1;
 static sonarHardware_t const *sonarHardware;
 
+#if !defined(UNIT_TEST)
 static void ECHO_EXTI_IRQHandler(void)
 {
     static uint32_t timing_start;
     uint32_t timing_stop;
 
-    if (digitalIn(GPIOB, sonarHardware->echo_pin) != 0) {
+    if (digitalIn(sonarHardware->echo_gpio, sonarHardware->echo_pin) != 0) {
         timing_start = micros();
     } else {
         timing_stop = micros();
@@ -72,14 +73,19 @@ void EXTI9_5_IRQHandler(void)
 {
     ECHO_EXTI_IRQHandler();
 }
+#endif
 
-void hcsr04_init(const sonarHardware_t *initialSonarHardware)
+void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sonarRange)
 {
+    sonarHardware = initialSonarHardware;
+    sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM;
+    sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
+    sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
+
+#if !defined(UNIT_TEST)
     gpio_config_t gpio;
     EXTI_InitTypeDef EXTIInit;
 
-    sonarHardware = initialSonarHardware;
-
 #ifdef STM32F10X
     // enable AFIO for EXTI support
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
@@ -96,12 +102,12 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware)
     gpio.pin = sonarHardware->trigger_pin;
     gpio.mode = Mode_Out_PP;
     gpio.speed = Speed_2MHz;
-    gpioInit(GPIOB, &gpio);
+    gpioInit(sonarHardware->trigger_gpio, &gpio);
 
     // echo pin
     gpio.pin = sonarHardware->echo_pin;
     gpio.mode = Mode_IN_FLOATING;
-    gpioInit(GPIOB, &gpio);
+    gpioInit(sonarHardware->echo_gpio, &gpio);
 
 #ifdef STM32F10X
     // setup external interrupt on echo pin
@@ -129,11 +135,15 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware)
     NVIC_Init(&NVIC_InitStructure);
 
     lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
+#else
+    lastMeasurementAt = 0; // to avoid "unused" compiler warning
+#endif
 }
 
 // measurement reading is done asynchronously, using interrupt
 void hcsr04_start_reading(void)
 {
+#if !defined(UNIT_TEST)
     uint32_t now = millis();
 
     if (now < (lastMeasurementAt + 60)) {
@@ -144,15 +154,15 @@ void hcsr04_start_reading(void)
 
     lastMeasurementAt = now;
 
-    digitalHi(GPIOB, sonarHardware->trigger_pin);
+    digitalHi(sonarHardware->trigger_gpio, sonarHardware->trigger_pin);
     //  The width of trig signal must be greater than 10us
     delayMicroseconds(11);
-    digitalLo(GPIOB, sonarHardware->trigger_pin);
+    digitalLo(sonarHardware->trigger_gpio, sonarHardware->trigger_pin);
+#endif
 }
 
 /**
- * Get the distance that was measured by the last pulse, in centimeters. When the ground is too far away to be
+ * Get the distance that was measured by the last pulse, in centimeters.
- * reliably read by the sonar, -1 is returned instead.
  */
 int32_t hcsr04_get_distance(void)
 {
@@ -163,10 +173,6 @@ int32_t hcsr04_get_distance(void)
     // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
     int32_t distance = measurement / 59;
 
-    // this sonar range is up to 4meter , but 3meter is the safe working range (+tilted and roll)
-    if (distance > 300)
-        distance = -1;
-
     return distance;
 }
 #endif

src/main/drivers/sonar_hcsr04.h

@@ -17,17 +17,31 @@
 
 #pragma once
 
+#include "platform.h"
+
 typedef struct sonarHardware_s {
     uint16_t trigger_pin;
+	GPIO_TypeDef* trigger_gpio;
     uint16_t echo_pin;
+	GPIO_TypeDef* echo_gpio;
     uint32_t exti_line;
     uint8_t exti_pin_source;
     IRQn_Type exti_irqn;
 } sonarHardware_t;
 
+typedef struct sonarRange_s {
+    int16_t maxRangeCm;
+    // these are full detection cone angles, maximum tilt is half of this
+    int16_t detectionConeDeciDegrees; // detection cone angle as in HC-SR04 device spec
+    int16_t detectionConeExtendedDeciDegrees; // device spec is conservative, in practice have slightly larger detection cone
+} sonarRange_t;
+
 #define SONAR_GPIO GPIOB
 
-void hcsr04_init(const sonarHardware_t *sonarHardware);
+#define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet
+#define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet
+#define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
 
+void hcsr04_init(const sonarHardware_t *sonarHardware, sonarRange_t *sonarRange);
 void hcsr04_start_reading(void);
 int32_t hcsr04_get_distance(void);

src/main/flight/altitudehold.c

@@ -30,6 +30,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/sonar_hcsr04.h"
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
@@ -173,16 +174,6 @@ bool isThrustFacingDownwards(attitudeEulerAngles_t *attitude)
     return ABS(attitude->values.roll) < DEGREES_80_IN_DECIDEGREES && ABS(attitude->values.pitch) < DEGREES_80_IN_DECIDEGREES;
 }
 
-/*
-* This (poorly named) function merely returns whichever is higher, roll inclination or pitch inclination.
-* //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft
-* (my best interpretation of scalar 'tiltAngle') or rename the function.
-*/
-int16_t calculateTiltAngle(attitudeEulerAngles_t *attitude)
-{
-    return MAX(ABS(attitude->values.roll), ABS(attitude->values.pitch));
-}
-
 int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
 {
     int32_t result = 0;
@@ -228,17 +219,15 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     float vel_acc;
     int32_t vel_tmp;
     float accZ_tmp;
-    int32_t sonarAlt = -1;
     static float accZ_old = 0.0f;
     static float vel = 0.0f;
     static float accAlt = 0.0f;
     static int32_t lastBaroAlt;
 
+#ifdef SONAR
+    int32_t sonarAlt = SONAR_OUT_OF_RANGE;
     static int32_t baroAlt_offset = 0;
     float sonarTransition;
-
-#ifdef SONAR
-    int16_t tiltAngle;
 #endif
 
     dTime = currentTime - previousTime;
@@ -260,21 +249,22 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 #endif
 
 #ifdef SONAR
-    tiltAngle = calculateTiltAngle(&attitude);
     sonarAlt = sonarRead();
-    sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
+    sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
-#endif
 
-    if (sonarAlt > 0 && sonarAlt < 200) {
+    if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
+        // just use the SONAR
         baroAlt_offset = BaroAlt - sonarAlt;
         BaroAlt = sonarAlt;
     } else {
         BaroAlt -= baroAlt_offset;
-        if (sonarAlt > 0  && sonarAlt <= 300) {
+        if (sonarAlt > 0  && sonarAlt <= sonarMaxAltWithTiltCm) {
-            sonarTransition = (300 - sonarAlt) / 100.0f;
+            // SONAR in range, so use complementary filter
+            sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
             BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
         }
     }
+#endif
 
     dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
 
@@ -305,12 +295,16 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     }
 #endif
 
-    if (sonarAlt > 0 && sonarAlt < 200) {
+#ifdef SONAR
+    if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
         // the sonar has the best range
         EstAlt = BaroAlt;
     } else {
         EstAlt = accAlt;
     }
+#else
+    EstAlt = accAlt;
+#endif
 
     baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
     lastBaroAlt = BaroAlt;
@@ -337,4 +331,3 @@ int32_t altitudeHoldGetEstimatedAltitude(void)
 }
 
 #endif
-

src/main/sensors/sonar.c

@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
 
 #include "platform.h"
 #include "build_config.h"
@@ -30,33 +31,48 @@
 #include "config/config.h"
 
 #include "sensors/sensors.h"
+#include "sensors/battery.h"
 #include "sensors/sonar.h"
 
-// in cm , -1 indicate sonar is not in range - inclination adjusted by imu
+// Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range.
+// Inclination is adjusted by imu
+    float baro_cf_vel;                      // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
+    float baro_cf_alt;                      // apply CF to use ACC for height estimation
 
 #ifdef SONAR
+int16_t sonarMaxRangeCm;
+int16_t sonarMaxAltWithTiltCm;
+int16_t sonarCfAltCm; // Complimentary Filter altitude
+STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees;
+float sonarMaxTiltCos;
 
 static int32_t calculatedAltitude;
 
-const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig) 
+const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
 {
 #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
-    static const sonarHardware_t sonarPWM56 = {
+    static const sonarHardware_t const sonarPWM56 = {
         .trigger_pin = Pin_8,   // PWM5 (PB8) - 5v tolerant
+        .trigger_gpio = GPIOB,
         .echo_pin = Pin_9,      // PWM6 (PB9) - 5v tolerant
+        .echo_gpio = GPIOB,
         .exti_line = EXTI_Line9,
         .exti_pin_source = GPIO_PinSource9,
         .exti_irqn = EXTI9_5_IRQn
     };
     static const sonarHardware_t sonarRC78 = {
         .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
+        .trigger_gpio = GPIOB,
         .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
+        .echo_gpio = GPIOB,
         .exti_line = EXTI_Line1,
         .exti_pin_source = GPIO_PinSource1,
         .exti_irqn = EXTI1_IRQn
     };
-    // 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 we are using softserial, parallel PWM 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) ) {
+    if (feature(FEATURE_SOFTSERIAL)
+            || feature(FEATURE_RX_PARALLEL_PWM )
+            || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
         return &sonarPWM56;
     } else {
         return &sonarRC78;
@@ -65,7 +81,9 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
     UNUSED(batteryConfig);
     static const sonarHardware_t const sonarHardware = {
         .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
+        .trigger_gpio = GPIOB,
         .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
+        .echo_gpio = GPIOB,
         .exti_line = EXTI_Line1,
         .exti_pin_source = GPIO_PinSource1,
         .exti_irqn = EXTI1_IRQn
@@ -75,22 +93,41 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
     UNUSED(batteryConfig);
     static const sonarHardware_t const sonarHardware = {
         .trigger_pin = Pin_5,   // (PB5)
+        .trigger_gpio = GPIOB,
         .echo_pin = Pin_0,      // (PB0) - only 3.3v ( add a 1K Ohms resistor )
+        .echo_gpio = GPIOB,
         .exti_line = EXTI_Line0,
         .exti_pin_source = GPIO_PinSource0,
         .exti_irqn = EXTI0_IRQn
     };
     return &sonarHardware;
-#elif defined(SPRACINGF3)
+#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI)
     UNUSED(batteryConfig);
     static const sonarHardware_t const sonarHardware = {
         .trigger_pin = Pin_0,   // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
+        .trigger_gpio = GPIOB,
         .echo_pin = Pin_1,      // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
+        .echo_gpio = GPIOB,
         .exti_line = EXTI_Line1,
         .exti_pin_source = EXTI_PinSource1,
         .exti_irqn = EXTI1_IRQn
     };
     return &sonarHardware;
+#elif defined(SPARKY)
+    UNUSED(batteryConfig);
+    static const sonarHardware_t const sonarHardware = {
+        .trigger_pin = Pin_2,   // PWM6 (PA2) - only 3.3v ( add a 1K Ohms resistor )
+        .trigger_gpio = GPIOA,
+        .echo_pin = Pin_1,      // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor )
+        .echo_gpio = GPIOB,
+        .exti_line = EXTI_Line1,
+        .exti_pin_source = EXTI_PinSource1,
+        .exti_irqn = EXTI1_IRQn
+    };
+    return &sonarHardware;
+#elif defined(UNIT_TEST)
+    UNUSED(batteryConfig);
+    return 0;
 #else
 #error Sonar not defined for target
 #endif
@@ -98,9 +135,42 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
 
 void sonarInit(const sonarHardware_t *sonarHardware)
 {
-    hcsr04_init(sonarHardware);
+    sonarRange_t sonarRange;
+
+    hcsr04_init(sonarHardware, &sonarRange);
     sensorsSet(SENSOR_SONAR);
-    calculatedAltitude = -1;
+    sonarMaxRangeCm = sonarRange.maxRangeCm;
+    sonarCfAltCm = sonarMaxRangeCm / 2;
+    sonarMaxTiltDeciDegrees =  sonarRange.detectionConeExtendedDeciDegrees / 2;
+    sonarMaxTiltCos = cos_approx(sonarMaxTiltDeciDegrees / 10.0f * RAD);
+    sonarMaxAltWithTiltCm = sonarMaxRangeCm * sonarMaxTiltCos;
+    calculatedAltitude = SONAR_OUT_OF_RANGE;
+}
+
+#define DISTANCE_SAMPLES_MEDIAN 5
+
+static int32_t applySonarMedianFilter(int32_t newSonarReading)
+{
+    static int32_t sonarFilterSamples[DISTANCE_SAMPLES_MEDIAN];
+    static int currentFilterSampleIndex = 0;
+    static bool medianFilterReady = false;
+    int nextSampleIndex;
+
+    if (newSonarReading > SONAR_OUT_OF_RANGE) // only accept samples that are in range
+    {
+        nextSampleIndex = (currentFilterSampleIndex + 1);
+        if (nextSampleIndex == DISTANCE_SAMPLES_MEDIAN) {
+            nextSampleIndex = 0;
+            medianFilterReady = true;
+        }
+
+        sonarFilterSamples[currentFilterSampleIndex] = newSonarReading;
+        currentFilterSampleIndex = nextSampleIndex;
+    }
+    if (medianFilterReady)
+        return quickMedianFilter5(sonarFilterSamples);
+    else
+        return newSonarReading;
 }
 
 void sonarUpdate(void)
@@ -109,32 +179,36 @@ void sonarUpdate(void)
 }
 
 /**
- * Get the last distance measured by the sonar in centimeters. When the ground is too far away, -1 is returned instead.
+ * Get the last distance measured by the sonar in centimeters. When the ground is too far away, SONAR_OUT_OF_RANGE is returned.
  */
 int32_t sonarRead(void)
 {
-    return hcsr04_get_distance();
+    int32_t distance = hcsr04_get_distance();
+    if (distance > HCSR04_MAX_RANGE_CM)
+        distance = SONAR_OUT_OF_RANGE;
+
+    return applySonarMedianFilter(distance);
 }
 
 /**
  * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
  * the altitude. Returns the computed altitude in centimeters.
  *
- * When the ground is too far away or the tilt is too strong, -1 is returned instead.
+ * When the ground is too far away or the tilt is too large, SONAR_OUT_OF_RANGE is returned.
  */
-int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
+int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle)
 {
-    // calculate sonar altitude only if the sonar is facing downwards(<25deg)
+    // calculate sonar altitude only if the ground is in the sonar cone
-    if (tiltAngle > 250)
+    if (cosTiltAngle <= sonarMaxTiltCos)
-        calculatedAltitude = -1;
+        calculatedAltitude = SONAR_OUT_OF_RANGE;
     else
-        calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
+        // altitude = distance * cos(tiltAngle), use approximation
-
+        calculatedAltitude = sonarDistance * cosTiltAngle;
     return calculatedAltitude;
 }
 
 /**
- * Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or -1 if sonarCalculateAltitude
+ * Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or SONAR_OUT_OF_RANGE if sonarCalculateAltitude
  * has never been called.
  */
 int32_t sonarGetLatestAltitude(void)

src/main/sensors/sonar.h

@@ -16,11 +16,14 @@
  */
 
 #pragma once
-#include "sensors/battery.h"
+
+#define SONAR_OUT_OF_RANGE (-1)
+
+extern int16_t sonarMaxRangeCm;
+extern int16_t sonarCfAltCm;
+extern int16_t sonarMaxAltWithTiltCm;
 
 void sonarUpdate(void);
-
 int32_t sonarRead(void);
-int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
+int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle);
 int32_t sonarGetLatestAltitude(void);
-