Fixed rangefinder reading.

src/main/drivers/rangefinder/rangefinder_hcsr04.c

@@ -25,19 +25,17 @@
 
 #if defined(USE_RANGEFINDER_HCSR04)
 
-#include "build/build_config.h"
-#include "build/debug.h"
-
 #include "common/time.h"
 
-#include "drivers/time.h"
 #include "drivers/exti.h"
 #include "drivers/io.h"
 #include "drivers/nvic.h"
 #include "drivers/rcc.h"
+#include "drivers/time.h"
 
 #include "drivers/rangefinder/rangefinder.h"
-#include "drivers/rangefinder/rangefinder_hcsr04.h"
+
+#include "rangefinder_hcsr04.h"
 
 #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
@@ -55,12 +53,10 @@
 
 static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
 static volatile timeMs_t lastMeasurementReceivedAt;
-static volatile int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
+static int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
 static timeMs_t lastMeasurementStartedAt = 0;
 
-#ifdef USE_EXTI
 static extiCallbackRec_t hcsr04_extiCallbackRec;
-#endif
 
 static IO_t echoIO;
 static IO_t triggerIO;
@@ -68,15 +64,18 @@ static IO_t triggerIO;
 #if !defined(UNIT_TEST)
 void hcsr04_extiHandler(extiCallbackRec_t* cb)
 {
-    static timeUs_t timing_start;
     UNUSED(cb);
 
+    static timeUs_t timing_start;
+
+    const timeUs_t currentTimeUs = microsISR();
+
     if (IORead(echoIO) != 0) {
-        timing_start = micros();
+        timing_start = currentTimeUs;
     } else {
-        const timeUs_t timing_stop = micros();
+        const timeUs_t timing_stop = currentTimeUs;
         if (timing_stop > timing_start) {
-            lastMeasurementReceivedAt = millis();
+            lastMeasurementReceivedAt = currentTimeUs / 1000;
             hcsr04SonarPulseTravelTime = timing_stop - timing_start;
         }
     }
@@ -88,7 +87,7 @@ void hcsr04_init(rangefinderDev_t *dev)
     UNUSED(dev);
 }
 
-#define HCSR04_MinimumFiringIntervalMs 60
+#define HCSR04_MINIMUM_FIRING_INTERVAL_MS 60
 
 /*
  * Start a range reading
@@ -98,28 +97,20 @@ void hcsr04_init(rangefinderDev_t *dev)
 void hcsr04_start_reading(void)
 {
 #if !defined(UNIT_TEST)
-#ifdef RANGEFINDER_HCSR04_TRIG_INVERTED
-    IOLo(triggerIO);
-    delayMicroseconds(11);
-    IOHi(triggerIO);
-#else
     IOHi(triggerIO);
     delayMicroseconds(11);
     IOLo(triggerIO);
 #endif
-#endif
 }
 
 void hcsr04_update(rangefinderDev_t *dev)
 {
     UNUSED(dev);
+
     const timeMs_t timeNowMs = millis();
 
-    // the firing interval of the trigger signal should be greater than 60ms
-    // to avoid interference between consecutive measurements
-    if (timeNowMs > lastMeasurementStartedAt + HCSR04_MinimumFiringIntervalMs) {
     // We should have a valid measurement within 60ms of trigger
-        if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MinimumFiringIntervalMs) {
+    if (lastMeasurementReceivedAt > lastMeasurementStartedAt && (lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
         // 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.
@@ -129,12 +120,14 @@ void hcsr04_update(rangefinderDev_t *dev)
         if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) {
             lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
         }
-        }
+    } else if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) > HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
-        else {
         // No measurement within reasonable time - indicate failure
         lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
     }
 
+    // the firing interval of the trigger signal should be greater than 60ms
+    // to avoid interference between consecutive measurements
+    if (timeNowMs > lastMeasurementStartedAt + HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
         // Trigger a new measurement
         lastMeasurementStartedAt = timeNowMs;
         hcsr04_start_reading();
@@ -147,30 +140,22 @@ void hcsr04_update(rangefinderDev_t *dev)
 int32_t hcsr04_get_distance(rangefinderDev_t *dev)
 {
     UNUSED(dev);
-    return lastCalculatedDistance;
+
+    const int32_t result = lastCalculatedDistance;
+
+    lastCalculatedDistance = RANGEFINDER_NO_NEW_DATA;
+
+    return result;
 }
 
 bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins)
 {
     bool detected = false;
 
-#ifdef STM32F10X
-    // enable AFIO for EXTI support
-    RCC_ClockCmd(RCC_APB2(AFIO), ENABLE);
-#endif
-
-#if defined(STM32F3) || defined(STM32F4)
-    RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this?
-#endif
-
     triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag);
     echoIO = IOGetByTag(rangefinderHardwarePins->echoTag);
 
-    if (IOGetOwner(triggerIO) != OWNER_FREE) {
+    if (IOGetOwner(triggerIO) || IOGetOwner(echoIO)) {
-        return false;
-    }
-
-    if (IOGetOwner(echoIO) != OWNER_FREE) {
         return false;
     }
 
@@ -190,7 +175,7 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa
             timeMs_t requestTime = millis();
             hcsr04_start_reading();
 
-            while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) {
+            while ((millis() - requestTime) < HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
                 if (IORead(echoIO) == true) {
                     detected = true;
                     break;
@@ -201,11 +186,9 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa
 
     if (detected) {
         // Hardware detected - configure the driver
-#ifdef USE_EXTI
         EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
         EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_BOTH); // TODO - priority!
         EXTIEnable(echoIO, true);
-#endif
 
         dev->delayMs = 100;
         dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
@@ -225,5 +208,4 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa
         return false;
     }
 }
-
 #endif

src/main/fc/tasks.c

@@ -189,6 +189,21 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
 }
 #endif
 
+#if defined(USE_RANGEFINDER)
+void taskUpdateRangefinder(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    if (!sensors(SENSOR_RANGEFINDER)) {
+        return;
+    }
+
+    rangefinderUpdate();
+
+    rangefinderProcess(getCosTiltAngle());
+}
+#endif
+
 #if defined(USE_BARO) || defined(USE_GPS)
 static void taskCalculateAltitude(timeUs_t currentTimeUs)
 {
@@ -476,7 +491,7 @@ task_t tasks[TASK_COUNT] = {
 #endif
 
 #ifdef USE_RANGEFINDER
-    [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, rangefinderUpdate, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
+    [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
 #endif
 };
 

src/main/sensors/rangefinder.c

@@ -254,16 +254,11 @@ static int16_t computePseudoSnr(int32_t newReading) {
 /*
  * This is called periodically by the scheduler
  */
-// XXX Returns timeDelta_t for iNav for pseudo-RT scheduling.
+void rangefinderUpdate(void)
-void rangefinderUpdate(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
-
     if (rangefinder.dev.update) {
         rangefinder.dev.update(&rangefinder.dev);
     }
-
-    // return rangefinder.dev.delayMs * 1000;  // to microseconds XXX iNav only
 }
 
 bool isSurfaceAltitudeValid() {
@@ -310,8 +305,7 @@ bool rangefinderProcess(float cosTiltAngle)
         if (distance >= 0) {
             rangefinder.lastValidResponseTimeMs = millis();
             rangefinder.rawAltitude = applyMedianFilter(distance);
-        }
+        } else if (distance == RANGEFINDER_OUT_OF_RANGE) {
-        else if (distance == RANGEFINDER_OUT_OF_RANGE) {
             rangefinder.lastValidResponseTimeMs = millis();
             rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
         }

src/main/sensors/rangefinder.h

@@ -56,14 +56,12 @@ typedef struct rangefinder_s {
     int16_t snr;
 } rangefinder_t;
 
-extern rangefinder_t rangefinder;
-
 void rangefinderResetDynamicThreshold(void);
 bool rangefinderInit(void);
 
 int32_t rangefinderGetLatestAltitude(void);
 int32_t rangefinderGetLatestRawAltitude(void);
 
-void rangefinderUpdate(timeUs_t currentTimeUs);
+void rangefinderUpdate(void);
 bool rangefinderProcess(float cosTiltAngle);
 bool rangefinderIsHealthy(void);

src/main/target/STM32_UNIFIED/target.h

@@ -210,7 +210,6 @@
 
 #define USE_TRANSPONDER
 
-//TODO: Make this actually work by making the pins configurable
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_HCSR04
 #define USE_RANGEFINDER_TF

src/main/target/common_post.h

@@ -378,6 +378,11 @@ extern uint8_t __config_end;
 #define USE_CUSTOM_DEFAULTS_ADDRESS
 #endif
 
+#if !defined(USE_EXTI)
+#undef USE_RX_SPI
+#undef USE_RANGEFINDER_HCSR04
+#endif
+
 #if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
 #define USE_RX_BIND
 #endif