Port iNav's rangefinder

make/source.mk

@@ -143,8 +143,8 @@ FC_SRC = \
             common/gps_conversion.c \
             drivers/display_ug2864hsweg01.c \
             drivers/light_ws2811strip.c \
+            drivers/rangefinder/rangefinder_hcsr04.c \
             drivers/serial_escserial.c \
-            drivers/sonar_hcsr04.c \
             drivers/vtx_common.c \
             flight/navigation.c \
             io/dashboard.c \
@@ -159,8 +159,8 @@ FC_SRC = \
             io/gps.c \
             io/ledstrip.c \
             io/osd.c \
-            sensors/sonar.c \
             sensors/barometer.c \
+            sensors/rangefinder.c \
             telemetry/telemetry.c \
             telemetry/crsf.c \
             telemetry/srxl.c \

src/main/blackbox/blackbox.c

@@ -69,7 +69,7 @@
 #include "sensors/battery.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
-#include "sensors/sonar.h"
+#include "sensors/rangefinder.h"
 
 enum {
     BLACKBOX_MODE_NORMAL = 0,
@@ -201,8 +201,8 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
 #ifdef USE_BARO
     {"BaroAlt",    -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
 #endif
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    {"sonarRaw",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SONAR},
+    {"surfaceRaw",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER},
 #endif
     {"rssi",       -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_RSSI},
 
@@ -303,8 +303,8 @@ typedef struct blackboxMainState_s {
 #ifdef USE_MAG
     int16_t magADC[XYZ_AXIS_COUNT];
 #endif
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    int32_t sonarRaw;
+    int32_t surfaceRaw;
 #endif
     uint16_t rssi;
 } blackboxMainState_t;
@@ -437,9 +437,9 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
     case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
         return (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) && (batteryConfig()->currentMeterSource != CURRENT_METER_VIRTUAL);
 
-    case FLIGHT_LOG_FIELD_CONDITION_SONAR:
+    case FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER:
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-        return feature(FEATURE_SONAR);
+        return sensors(SENSOR_RANGEFINDER);
 #else
         return false;
 #endif
@@ -568,9 +568,9 @@ static void writeIntraframe(void)
     }
 #endif
 
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
+    if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER)) {
-        blackboxWriteSignedVB(blackboxCurrent->sonarRaw);
+        blackboxWriteSignedVB(blackboxCurrent->surfaceRaw);
     }
 #endif
 
@@ -698,9 +698,9 @@ static void writeInterframe(void)
     }
 #endif
 
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
+    if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER)) {
-        deltas[optionalFieldCount++] = blackboxCurrent->sonarRaw - blackboxLast->sonarRaw;
+        deltas[optionalFieldCount++] = blackboxCurrent->surfaceRaw - blackboxLast->surfaceRaw;
     }
 #endif
 
@@ -1022,9 +1022,9 @@ static void loadMainState(timeUs_t currentTimeUs)
     blackboxCurrent->BaroAlt = baro.BaroAlt;
 #endif
 
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
     // Store the raw sonar value without applying tilt correction
-    blackboxCurrent->sonarRaw = sonarRead();
+    blackboxCurrent->surfaceRaw = rangefinderGetLatestAltitude();
 #endif
 
     blackboxCurrent->rssi = getRssi();

src/main/blackbox/blackbox_fielddefs.h

@@ -33,7 +33,7 @@ typedef enum FlightLogFieldCondition {
     FLIGHT_LOG_FIELD_CONDITION_BARO,
     FLIGHT_LOG_FIELD_CONDITION_VBAT,
     FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC,
-    FLIGHT_LOG_FIELD_CONDITION_SONAR,
+    FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER,
     FLIGHT_LOG_FIELD_CONDITION_RSSI,
 
     FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,

src/main/build/debug.c

@@ -56,4 +56,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "MAX7456_SPICLOCK",
     "SBUS",
     "FPORT",
+    "RANGEFINDER",
+    "RANGEFINDER_QUALITY",
 };

src/main/build/debug.h

@@ -74,6 +74,8 @@ typedef enum {
     DEBUG_MAX7456_SPICLOCK,
     DEBUG_SBUS,
     DEBUG_FPORT,
+    DEBUG_RANGEFINDER,
+    DEBUG_RANGEFINDER_QUALITY,
     DEBUG_COUNT
 } debugType_e;
 

src/main/drivers/rangefinder/rangefinder.h

@@ -17,24 +17,40 @@
 
 #pragma once
 
+#include "common/time.h"
 #include "drivers/io_types.h"
 
-typedef struct sonarConfig_s {
+#define RANGEFINDER_OUT_OF_RANGE        (-1)
+#define RANGEFINDER_HARDWARE_FAILURE    (-2)
+#define RANGEFINDER_NO_NEW_DATA         (-3)
+
+struct rangefinderDev_s;
+
+typedef struct rangefinderHardwarePins_s {
     ioTag_t triggerTag;
     ioTag_t echoTag;
-} sonarConfig_t;
+} rangefinderHardwarePins_t;
 
-typedef struct sonarRange_s {
+struct rangefinderDev_s;
+
+typedef void (*rangefinderOpInitFuncPtr)(struct rangefinderDev_s * dev);
+typedef void (*rangefinderOpStartFuncPtr)(struct rangefinderDev_s * dev);
+typedef int32_t (*rangefinderOpReadFuncPtr)(struct rangefinderDev_s * dev);
+
+typedef struct rangefinderDev_s {
+    timeMs_t delayMs;
     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 detectionConeDeciDegrees; // detection cone angle as in device spec
     int16_t detectionConeExtendedDeciDegrees; // device spec is conservative, in practice have slightly larger detection cone
-} sonarRange_t;
 
-#define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet
+    // function pointers
-#define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet
+    rangefinderOpInitFuncPtr init;
-#define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
+    rangefinderOpStartFuncPtr update;
+    rangefinderOpReadFuncPtr read;
+} rangefinderDev_t;
 
-void hcsr04_init(const sonarConfig_t *sonarConfig, sonarRange_t *sonarRange);
+extern int16_t rangefinderMaxRangeCm;
-void hcsr04_start_reading(void);
+extern int16_t rangefinderMaxAltWithTiltCm;
-int32_t hcsr04_get_distance(void);
+extern int16_t rangefinderCfAltCm;

src/main/drivers/rangefinder/rangefinder_hcsr04.c

@@ -0,0 +1,226 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#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/rangefinder/rangefinder.h"
+#include "drivers/rangefinder/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
+#define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
+
+
+/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
+ * When triggered it sends out a series of 40KHz ultrasonic pulses and receives
+ * 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 ***
+ *
+ */
+
+static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
+static volatile timeMs_t lastMeasurementReceivedAt;
+static volatile 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;
+
+#if !defined(UNIT_TEST)
+void hcsr04_extiHandler(extiCallbackRec_t* cb)
+{
+    static timeUs_t timing_start;
+    UNUSED(cb);
+
+    if (IORead(echoIO) != 0) {
+        timing_start = micros();
+    } else {
+        const timeUs_t timing_stop = micros();
+        if (timing_stop > timing_start) {
+            lastMeasurementReceivedAt = millis();
+            hcsr04SonarPulseTravelTime = timing_stop - timing_start;
+        }
+    }
+}
+#endif
+
+void hcsr04_init(rangefinderDev_t *dev)
+{
+    UNUSED(dev);
+}
+
+#define HCSR04_MinimumFiringIntervalMs 60
+
+/*
+ * Start a range reading
+ * Called periodically by the scheduler
+ * Measurement reading is done asynchronously, using interrupt
+ */
+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) {
+            // 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
+
+            lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59;
+            if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) {
+                lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
+            }
+        }
+        else {
+            // No measurement within reasonable time - indicate failure
+            lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
+        }
+
+        // Trigger a new measurement
+        lastMeasurementStartedAt = timeNowMs;
+        hcsr04_start_reading();
+    }
+}
+
+/**
+ * Get the distance that was measured by the last pulse, in centimeters.
+ */
+int32_t hcsr04_get_distance(rangefinderDev_t *dev)
+{
+    UNUSED(dev);
+    return lastCalculatedDistance;
+}
+
+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) {
+        return false;
+    }
+
+    if (IOGetOwner(echoIO) != OWNER_FREE) {
+        return false;
+    }
+
+    // trigger pin
+    IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0);
+    IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
+    IOLo(triggerIO);
+    delay(100);
+
+    // echo pin
+    IOInit(echoIO, OWNER_SONAR_ECHO, 0);
+    IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
+
+    // HC-SR04 echo line should be low by default and should return a response pulse when triggered
+    if (IORead(echoIO) == false) {
+        for (int i = 0; i < 5 && !detected; i++) {
+            timeMs_t requestTime = millis();
+            hcsr04_start_reading();
+
+            while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) {
+                if (IORead(echoIO) == true) {
+                    detected = true;
+                    break;
+                }
+            }
+        }
+    }
+
+    if (detected) {
+        // Hardware detected - configure the driver
+#ifdef USE_EXTI
+        EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
+        EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
+        EXTIEnable(echoIO, true);
+#endif
+
+        dev->delayMs = 100;
+        dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
+        dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
+        dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
+
+        dev->init = &hcsr04_init;
+        dev->update = &hcsr04_update;
+        dev->read = &hcsr04_get_distance;
+
+        return true;
+    }
+    else {
+        // Not detected - free resources
+        IORelease(triggerIO);
+        IORelease(echoIO);
+        return false;
+    }
+}
+
+#endif

src/main/drivers/rangefinder/rangefinder_hcsr04.h

@@ -19,19 +19,16 @@
 
 #include "pg/pg.h"
 #include "common/time.h"
-#include "drivers/sonar_hcsr04.h"
+#include "drivers/rangefinder/rangefinder.h"
 #include "sensors/battery.h"
 
-#define SONAR_OUT_OF_RANGE (-1)
+#define RANGEFINDER_HCSR04_TASK_PERIOD_MS 70
 
-extern int16_t sonarMaxRangeCm;
+typedef struct sonarConfig_s {
-extern int16_t sonarCfAltCm;
+    ioTag_t triggerTag;
-extern int16_t sonarMaxAltWithTiltCm;
+    ioTag_t echoTag;
+} sonarConfig_t;
 
 PG_DECLARE(sonarConfig_t, sonarConfig);
 
-void sonarInit(const sonarConfig_t *sonarConfig);
+bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * sonarConfig);
-void sonarUpdate(timeUs_t currentTimeUs);
-int32_t sonarRead(void);
-int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle);
-int32_t sonarGetLatestAltitude(void);

src/main/drivers/resource.c

@@ -68,4 +68,5 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
     "ESCSERIAL",
     "CAMERA_CONTROL",
     "TIMUP",
+    "RANGEFINDER",
 };

src/main/drivers/resource.h

@@ -68,6 +68,7 @@ typedef enum {
     OWNER_ESCSERIAL,
     OWNER_CAMERA_CONTROL,
     OWNER_TIMUP,
+    OWNER_RANGEFINDER,
     OWNER_TOTAL_COUNT
 } resourceOwner_e;
 

src/main/drivers/sonar_hcsr04.c

@@ -1,142 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-
-#include "platform.h"
-
-#if defined(USE_SONAR)
-
-#include "build/build_config.h"
-
-#include "drivers/exti.h"
-#include "drivers/io.h"
-#include "drivers/nvic.h"
-#include "drivers/sonar_hcsr04.h"
-#include "drivers/time.h"
-
-/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
- * When triggered it sends out a series of 40KHz ultrasonic pulses and receives
- * 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 ***
- *
- */
-
-STATIC_UNIT_TESTED volatile int32_t measurement = -1;
-static uint32_t lastMeasurementAt;
-
-extiCallbackRec_t hcsr04_extiCallbackRec;
-
-static IO_t echoIO;
-static IO_t triggerIO;
-
-void hcsr04_extiHandler(extiCallbackRec_t* cb)
-{
-    static uint32_t timing_start;
-    uint32_t timing_stop;
-    UNUSED(cb);
-
-    if (IORead(echoIO) != 0) {
-        timing_start = micros();
-    }
-    else {
-        timing_stop = micros();
-        if (timing_stop > timing_start) {
-            measurement = timing_stop - timing_start;
-        }
-    }
-}
-
-void hcsr04_init(const sonarConfig_t *sonarConfig, sonarRange_t *sonarRange)
-{
-    sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM;
-    sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
-    sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
-
-#if !defined(UNIT_TEST)
-
-#ifdef STM32F10X
-    // enable AFIO for EXTI support
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
-#endif
-
-#if defined(STM32F3) || defined(STM32F4)
-    /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
-#endif
-
-    // trigger pin
-    triggerIO = IOGetByTag(sonarConfig->triggerTag);
-    IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0);
-    IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
-
-    // echo pin
-    echoIO = IOGetByTag(sonarConfig->echoTag);
-    IOInit(echoIO, OWNER_SONAR_ECHO, 0);
-    IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
-
-#ifdef USE_EXTI
-    EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
-    EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
-    EXTIEnable(echoIO, true);
-#endif
-
-    lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
-#else
-    UNUSED(lastMeasurementAt); // 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)) {
-        // the repeat interval of trig signal should be greater than 60ms
-        // to avoid interference between connective measurements.
-        return;
-    }
-
-    lastMeasurementAt = now;
-
-    IOHi(triggerIO);
-    //  The width of trig signal must be greater than 10us
-    delayMicroseconds(11);
-    IOLo(triggerIO);
-#endif
-}
-
-/**
- * Get the distance that was measured by the last pulse, in centimeters.
- */
-int32_t hcsr04_get_distance(void)
-{
-    // 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 distance = measurement / 59;
-
-    return distance;
-}
-#endif

src/main/fc/config.c

@@ -53,7 +53,6 @@
 #include "drivers/rx_spi.h"
 #include "drivers/sdcard.h"
 #include "drivers/sensor.h"
-#include "drivers/sonar_hcsr04.h"
 #include "drivers/sound_beeper.h"
 #include "drivers/system.h"
 #include "drivers/timer.h"
@@ -403,8 +402,8 @@ static void validateAndFixConfig(void)
     featureClear(FEATURE_GPS);
 #endif
 
-#ifndef USE_SONAR
+#ifndef USE_RANGEFINDER
-    featureClear(FEATURE_SONAR);
+    featureClear(FEATURE_RANGEFINDER);
 #endif
 
 #ifndef USE_TELEMETRY

src/main/fc/config.h

@@ -38,7 +38,7 @@ typedef enum {
     FEATURE_SERVO_TILT = 1 << 5,
     FEATURE_SOFTSERIAL = 1 << 6,
     FEATURE_GPS = 1 << 7,
-    FEATURE_SONAR = 1 << 9,
+    FEATURE_RANGEFINDER = 1 << 9,
     FEATURE_TELEMETRY = 1 << 10,
     FEATURE_3D = 1 << 12,
     FEATURE_RX_PARALLEL_PWM = 1 << 13,

src/main/fc/fc_core.c

@@ -627,8 +627,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 #if defined(USE_ALT_HOLD)
     // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
     updateRcCommands();
-    if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
+    if (sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER)) {
-        if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
+        if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE)) {
             applyAltHold();
         }
     }

src/main/fc/fc_init.c

@@ -60,7 +60,6 @@
 #include "drivers/buttons.h"
 #include "drivers/inverter.h"
 #include "drivers/flash_m25p16.h"
-#include "drivers/sonar_hcsr04.h"
 #include "drivers/sdcard.h"
 #include "drivers/usb_io.h"
 #include "drivers/transponder_ir.h"
@@ -121,7 +120,6 @@
 #include "sensors/gyro.h"
 #include "sensors/initialisation.h"
 #include "sensors/sensors.h"
-#include "sensors/sonar.h"
 
 #include "telemetry/telemetry.h"
 
@@ -473,14 +471,16 @@ void init(void)
     cameraControlInit();
 #endif
 
-#if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(USE_SONAR) && defined(USE_SOFTSERIAL2)
+// XXX These kind of code should goto target/config.c?
-    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
+// XXX And these no longer work properly as FEATURE_RANGEFINDER does control HCSR04 runtime configuration.
+#if defined(RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2)
+    if (feature(FEATURE_RANGEFINDER) && feature(FEATURE_SOFTSERIAL)) {
         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
     }
 #endif
 
-#if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(USE_SONAR) && defined(USE_SOFTSERIAL1)
+#if defined(RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1)
-    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
+    if (feature(FEATURE_RANGEFINDER) && feature(FEATURE_SOFTSERIAL)) {
         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
     }
 #endif

src/main/fc/fc_tasks.c

@@ -76,8 +76,8 @@
 #include "sensors/compass.h"
 #include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
+#include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
-#include "sensors/sonar.h"
 
 #include "scheduler/scheduler.h"
 
@@ -87,10 +87,6 @@
 void taskBstMasterProcess(timeUs_t currentTimeUs);
 #endif
 
-#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
-#define TASK_PERIOD_MS(ms) ((ms) * 1000)
-#define TASK_PERIOD_US(us) (us)
-
 bool taskSerialCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
 {
     UNUSED(currentTimeUs);
@@ -151,9 +147,9 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
     }
 #endif
 
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    if (sensors(SENSOR_SONAR)) {
+    if (sensors(SENSOR_RANGEFINDER)) {
-        updateSonarAltHoldState();
+        updateRangefinderAltHoldState();
     }
 #endif
 #endif // USE_ALT_HOLD
@@ -183,20 +179,20 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
 }
 #endif
 
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
 static void taskCalculateAltitude(timeUs_t currentTimeUs)
 {
     if (false
 #if defined(USE_BARO)
         || (sensors(SENSOR_BARO) && isBaroReady())
 #endif
-#if defined(USE_SONAR)
+#if defined(USE_RANGEFINDER)
-        || sensors(SENSOR_SONAR)
+        || sensors(SENSOR_RANGEFINDER)
 #endif
         ) {
         calculateEstimatedAltitude(currentTimeUs);
     }}
-#endif // USE_BARO || USE_SONAR
+#endif // USE_BARO || USE_RANGEFINDER
 
 #ifdef USE_TELEMETRY
 static void taskTelemetry(timeUs_t currentTimeUs)
@@ -292,11 +288,11 @@ void fcTasksInit(void)
 #ifdef USE_BARO
     setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
 #endif
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
+    setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
 #endif
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-    setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
+    setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER));
 #endif
 #ifdef USE_DASHBOARD
     setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
@@ -492,16 +488,16 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    [TASK_SONAR] = {
+    [TASK_RANGEFINDER] = {
-        .taskName = "SONAR",
+        .taskName = "RANGEFINDER",
-        .taskFunc = sonarUpdate,
+        .taskFunc = rangefinderUpdate,
-        .desiredPeriod = TASK_PERIOD_MS(70),        // 70ms required so that SONAR pulses do not interfere with each other
+        .desiredPeriod = TASK_PERIOD_MS(70), // XXX HCSR04 sonar specific value.
         .staticPriority = TASK_PRIORITY_LOW,
     },
 #endif
 
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
     [TASK_ALTITUDE] = {
         .taskName = "ALTITUDE",
         .taskFunc = taskCalculateAltitude,

src/main/fc/fc_tasks.h

@@ -19,4 +19,8 @@
 
 #define LOOPTIME_SUSPEND_TIME 3  // Prevent too long busy wait times
 
+#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
+#define TASK_PERIOD_MS(ms) ((ms) * 1000)
+#define TASK_PERIOD_US(us) (us)
+
 void fcTasksInit(void);

src/main/fc/rc_modes.h

@@ -44,7 +44,7 @@ typedef enum {
     BOXOSD,
     BOXTELEMETRY,
     BOXGTUNE,
-    BOXSONAR,
+    BOXRANGEFINDER,
     BOXSERVO1,
     BOXSERVO2,
     BOXSERVO3,

src/main/fc/runtime_config.h

@@ -75,7 +75,7 @@ typedef enum {
     HEADFREE_MODE   = (1 << 6),
     UNUSED_MODE     = (1 << 7), // old autotune
     PASSTHRU_MODE   = (1 << 8),
-    SONAR_MODE      = (1 << 9),
+    RANGEFINDER_MODE= (1 << 9),
     FAILSAFE_MODE   = (1 << 10)
 } flightModeFlags_e;
 
@@ -90,7 +90,7 @@ extern uint16_t flightModeFlags;
 // It is much more memory efficient than full map (uint32_t -> uint8_t)
 #define FLIGHT_MODE_BOXID_MAP_INITIALIZER {                             \
         BOXANGLE, BOXHORIZON, BOXMAG, BOXBARO, BOXGPSHOME, BOXGPSHOLD,  \
-        BOXHEADFREE, -1, BOXPASSTHRU, BOXSONAR, BOXFAILSAFE}  \
+        BOXHEADFREE, -1, BOXPASSTHRU, BOXRANGEFINDER, BOXFAILSAFE}  \
         /**/
 
 typedef enum {

src/main/flight/altitude.c

@@ -44,7 +44,7 @@
 
 #include "sensors/sensors.h"
 #include "sensors/barometer.h"
-#include "sensors/sonar.h"
+#include "sensors/rangefinder.h"
 
 
 int32_t AltHold;
@@ -146,16 +146,16 @@ void updateAltHoldState(void)
     }
 }
 
-void updateSonarAltHoldState(void)
+void updateRangefinderAltHoldState(void)
 {
     // Sonar alt hold activate
-    if (!IS_RC_MODE_ACTIVE(BOXSONAR)) {
+    if (!IS_RC_MODE_ACTIVE(BOXRANGEFINDER)) {
-        DISABLE_FLIGHT_MODE(SONAR_MODE);
+        DISABLE_FLIGHT_MODE(RANGEFINDER_MODE);
         return;
     }
 
-    if (!FLIGHT_MODE(SONAR_MODE)) {
+    if (!FLIGHT_MODE(RANGEFINDER_MODE)) {
-        ENABLE_FLIGHT_MODE(SONAR_MODE);
+        ENABLE_FLIGHT_MODE(RANGEFINDER_MODE);
         AltHold = estimatedAltitude;
         initialThrottleHold = rcData[THROTTLE];
         errorVelocityI = 0;
@@ -205,7 +205,7 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
 }
 #endif // USE_ALT_HOLD
 
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
 void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 {
     static timeUs_t previousTimeUs = 0;
@@ -232,14 +232,14 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
     }
 #endif
 
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    if (sensors(SENSOR_SONAR)) {
+    if (sensors(SENSOR_RANGEFINDER) && rangefinderProcess(getCosTiltAngle())) {
-        int32_t sonarAlt = sonarCalculateAltitude(sonarRead(), getCosTiltAngle());
+        int32_t rangefinderAlt = rangefinderGetLatestAltitude();
-        if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
+        if (rangefinderAlt > 0 && rangefinderAlt >= rangefinderCfAltCm && rangefinderAlt <= rangefinderMaxAltWithTiltCm) {
-            // SONAR in range, so use complementary filter
+            // RANGEFINDER in range, so use complementary filter
-            float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
+            float rangefinderTransition = (float)(rangefinderMaxAltWithTiltCm - rangefinderAlt) / (rangefinderMaxAltWithTiltCm - rangefinderCfAltCm);
-            sonarAlt = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
+            rangefinderAlt = (float)rangefinderAlt * rangefinderTransition + baroAlt * (1.0f - rangefinderTransition);
-            estimatedAltitude = sonarAlt;
+            estimatedAltitude = rangefinderAlt;
         }
     }
 #endif
@@ -301,7 +301,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
     UNUSED(accZ_tmp);
 #endif
 }
-#endif // USE_BARO || USE_SONAR
+#endif // USE_BARO || USE_RANGEFINDER
 
 int32_t getEstimatedAltitude(void)
 {

src/main/flight/altitude.h

@@ -33,4 +33,4 @@ int32_t getEstimatedVario(void);
 
 void applyAltHold(void);
 void updateAltHoldState(void);
-void updateSonarAltHoldState(void);
+void updateRangefinderAltHoldState(void);

src/main/flight/imu.c

@@ -46,7 +46,6 @@
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
-#include "sensors/sonar.h"
 
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
 #include <stdio.h>

src/main/interface/cli.c

@@ -72,7 +72,7 @@ extern uint8_t __config_end;
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
 #include "drivers/serial_escserial.h"
-#include "drivers/sonar_hcsr04.h"
+#include "drivers/rangefinder/rangefinder_hcsr04.h"
 #include "drivers/stack_check.h"
 #include "drivers/system.h"
 #include "drivers/transponder_ir.h"
@@ -175,7 +175,7 @@ static const char * const mixerNames[] = {
 static const char * const featureNames[] = {
     "RX_PPM", "", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
     "SERVO_TILT", "SOFTSERIAL", "GPS", "",
-    "SONAR", "TELEMETRY", "", "3D", "RX_PARALLEL_PWM",
+    "RANGEFINDER", "TELEMETRY", "", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
     "", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
     "", "", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL
@@ -192,13 +192,13 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT
 #if defined(USE_SENSOR_NAMES)
 // sync this with sensors_e
 static const char * const sensorTypeNames[] = {
-    "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
+    "GYRO", "ACC", "BARO", "MAG", "RANGEFINDER", "GPS", "GPS+MAG", NULL
 };
 
-#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
+#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER)
 
 static const char * const *sensorHardwareNames[] = {
-    lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware
+    lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware, lookupTableRangefinderHardware
 };
 #endif // USE_SENSOR_NAMES
 
@@ -2031,8 +2031,8 @@ static void cliFeature(char *cmdline)
                     break;
                 }
 #endif
-#ifndef USE_SONAR
+#ifndef USE_RANGEFINDER
-                if (mask & FEATURE_SONAR) {
+                if (mask & FEATURE_RANGEFINDER) {
                     cliPrintLine("unavailable");
                     break;
                 }
@@ -3166,7 +3166,7 @@ const cliResourceValue_t resourceTable[] = {
     { OWNER_PPMINPUT,      PG_PPM_CONFIG, offsetof(ppmConfig_t, ioTag), 0 },
     { OWNER_PWMINPUT,      PG_PWM_CONFIG, offsetof(pwmConfig_t, ioTags[0]), PWM_INPUT_PORT_COUNT },
 #endif
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER_HCSR04
     { OWNER_SONAR_TRIGGER, PG_SONAR_CONFIG, offsetof(sonarConfig_t, triggerTag), 0 },
     { OWNER_SONAR_ECHO,    PG_SONAR_CONFIG, offsetof(sonarConfig_t, echoTag),    0 },
 #endif

src/main/interface/msp.c

@@ -107,11 +107,11 @@
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/boardalignment.h"
+#include "sensors/esc_sensor.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
+#include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
-#include "sensors/sonar.h"
-#include "sensors/esc_sensor.h"
 
 #include "telemetry/telemetry.h"
 
@@ -733,7 +733,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
 #else
             sbufWriteU16(dst, 0);
 #endif
-            sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4 | sensors(SENSOR_GYRO) << 5);
+            sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5);
             sbufWriteData(dst, &flightModeFlags, 4);        // unconditional part of flags, first 32 bits
             sbufWriteU8(dst, getCurrentPidProfileIndex());
             sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
@@ -849,7 +849,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         break;
 
     case MSP_ALTITUDE:
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
         sbufWriteU32(dst, getEstimatedAltitude());
 #else
         sbufWriteU32(dst, 0);
@@ -858,8 +858,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         break;
 
     case MSP_SONAR_ALTITUDE:
-#if defined(USE_SONAR)
+#if defined(USE_RANGEFINDER)
-        sbufWriteU32(dst, sonarGetLatestAltitude());
+        sbufWriteU32(dst, rangefinderGetLatestAltitude());
 #else
         sbufWriteU32(dst, 0);
 #endif

src/main/interface/msp_box.c

@@ -64,7 +64,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXOSD, "OSD DISABLE SW", 19 },
     { BOXTELEMETRY, "TELEMETRY", 20 },
     { BOXGTUNE, "GTUNE", 21 },
-    { BOXSONAR, "SONAR", 22 },
+    { BOXRANGEFINDER, "RANGEFINDER", 22 },
     { BOXSERVO1, "SERVO1", 23 },
     { BOXSERVO2, "SERVO2", 24 },
     { BOXSERVO3, "SERVO3", 25 },
@@ -188,9 +188,9 @@ void initActiveBoxIds(void)
     }
 #endif
 
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    if (feature(FEATURE_SONAR)) {
+    if (feature(FEATURE_RANGEFINDER)) { // XXX && sensors(SENSOR_RANGEFINDER)?
-        BME(BOXSONAR);
+        BME(BOXRANGEFINDER);
     }
 #endif
 

src/main/interface/settings.c

@@ -78,6 +78,7 @@
 #include "sensors/compass.h"
 #include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
+#include "sensors/rangefinder.h"
 
 #include "telemetry/frsky.h"
 #include "telemetry/telemetry.h"
@@ -109,6 +110,11 @@ const char * const lookupTableMagHardware[] = {
     "AUTO", "NONE", "HMC5883", "AK8975", "AK8963"
 };
 #endif
+#if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)
+const char * const lookupTableRangefinderHardware[] = {
+    "NONE", "HCSR04"
+};
+#endif
 
 static const char * const lookupTableOffOn[] = {
     "OFF", "ON"
@@ -318,6 +324,9 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_MAX7456
     { lookupTableMax7456Clock, sizeof(lookupTableMax7456Clock) / sizeof(char *) },
 #endif
+#ifdef USE_RANGEFINDER
+    { lookupTableRangefinderHardware, sizeof(lookupTableRangefinderHardware) / sizeof(char *) },
+#endif
 };
 
 const clivalue_t valueTable[] = {
@@ -829,6 +838,11 @@ const clivalue_t valueTable[] = {
     { "camera_control_key_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, keyDelayMs) },
     { "camera_control_internal_resistance", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 1000 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, internalResistance) },
 #endif
+
+// PG_RANGEFINDER_CONFIG
+#ifdef USE_RANGEFINDER
+    { "rangefinder_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) },
+#endif
 };
 
 const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

src/main/interface/settings.h

@@ -71,8 +71,12 @@ typedef enum {
     TABLE_BUS_TYPE,
 #ifdef USE_MAX7456
     TABLE_MAX7456_CLOCK,
+#endif
+#ifdef USE_RANGEFINDER
+    TABLE_RANGEFINDER_HARDWARE,
 #endif
     LOOKUP_TABLE_COUNT
+
 } lookupTableIndex_e;
 
 typedef struct lookupTableEntry_s {
@@ -153,3 +157,5 @@ extern const char * const lookupTableBaroHardware[];
 
 extern const char * const lookupTableMagHardware[];
 //extern const uint8_t lookupTableMagHardwareEntryCount;
+
+extern const char * const lookupTableRangefinderHardware[];

src/main/pg/pg_ids.h

@@ -117,7 +117,8 @@
 #define PG_MAX7456_CONFIG 524
 #define PG_FLYSKY_CONFIG 525
 #define PG_TIME_CONFIG 526
-#define PG_BETAFLIGHT_END 526
+#define PG_RANGEFINDER_CONFIG 527 // iNav
+#define PG_BETAFLIGHT_END 527
 
 
 // OSD configuration (subject to change)

src/main/scheduler/scheduler.h

@@ -71,10 +71,10 @@ typedef enum {
 #ifdef USE_BARO
     TASK_BARO,
 #endif
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    TASK_SONAR,
+    TASK_RANGEFINDER,
 #endif
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
     TASK_ALTITUDE,
 #endif
 #ifdef USE_DASHBOARD

src/main/sensors/initialisation.c

@@ -35,24 +35,12 @@
 #include "sensors/barometer.h"
 #include "sensors/gyro.h"
 #include "sensors/compass.h"
-#include "sensors/sonar.h"
+#include "sensors/rangefinder.h"
 #include "sensors/initialisation.h"
 
-uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
+// requestedSensors is not actually used
-
+uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
-
+uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
-#ifdef USE_SONAR
-static bool sonarDetect(void)
-{
-    if (feature(FEATURE_SONAR)) {
-        // the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
-        // since there is no way to detect it
-        sensorsSet(SENSOR_SONAR);
-        return true;
-    }
-    return false;
-}
-#endif
 
 bool sensorsAutodetect(void)
 {
@@ -73,10 +61,8 @@ bool sensorsAutodetect(void)
     baroDetect(&baro.dev, barometerConfig()->baro_hardware);
 #endif
 
-#ifdef USE_SONAR
+#ifdef USE_RANGEFINDER
-    if (sonarDetect()) {
+    rangefinderInit();
-        sonarInit(sonarConfig());
-    }
 #endif
 
     return gyroDetected;

src/main/sensors/rangefinder.c

@@ -0,0 +1,370 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include <platform.h>
+
+#ifdef USE_RANGEFINDER
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "config/feature.h"
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "drivers/io.h"
+#include "drivers/time.h"
+#include "drivers/rangefinder/rangefinder_hcsr04.h"
+#include "drivers/rangefinder/rangefinder.h"
+
+#include "fc/config.h"
+#include "fc/runtime_config.h"
+#include "fc/fc_tasks.h"
+
+#include "sensors/sensors.h"
+#include "sensors/rangefinder.h"
+#include "sensors/battery.h"
+
+#include "scheduler/scheduler.h"
+
+//#include "uav_interconnect/uav_interconnect.h"
+
+// XXX Interface to CF/BF legacy(?) altitude estimation code.
+// XXX Will be gone once iNav's estimator is ported.
+int16_t rangefinderMaxRangeCm;
+int16_t rangefinderMaxAltWithTiltCm;
+int16_t rangefinderCfAltCm; // Complimentary Filter altitude
+
+rangefinder_t rangefinder;
+
+#define RANGEFINDER_HARDWARE_TIMEOUT_MS         500     // Accept 500ms of non-responsive sensor, report HW failure otherwise
+
+#define RANGEFINDER_DYNAMIC_THRESHOLD           600     //Used to determine max. usable rangefinder disatance
+#define RANGEFINDER_DYNAMIC_FACTOR              75    
+
+PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
+
+PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
+    .rangefinder_hardware = RANGEFINDER_NONE,
+);
+
+#ifdef USE_RANGEFINDER_HCSR04
+PG_REGISTER_WITH_RESET_TEMPLATE(sonarConfig_t, sonarConfig, PG_SONAR_CONFIG, 1);
+
+#ifndef RANGEFINDER_HCSR04_TRIGGER_PIN
+#define RANGEFINDER_HCSR04_TRIGGER_PIN NONE
+#endif
+#ifndef RANGEFINDER_HCSR04_ECHO_PIN
+#define RANGEFINDER_HCSR04_ECHO_PIN NONE
+#endif
+
+PG_RESET_TEMPLATE(sonarConfig_t, sonarConfig,
+    .triggerTag = IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN),
+    .echoTag = IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN),
+);
+#endif
+
+/*
+ * Detect which rangefinder is present
+ */
+static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwareToUse)
+{
+    rangefinderType_e rangefinderHardware = RANGEFINDER_NONE;
+    requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardwareToUse;
+
+    switch (rangefinderHardwareToUse) {
+        case RANGEFINDER_HCSR04:
+#ifdef USE_RANGEFINDER_HCSR04
+            {
+                if (hcsr04Detect(dev, sonarConfig())) {   // FIXME: Do actual detection if HC-SR04 is plugged in
+                    rangefinderHardware = RANGEFINDER_HCSR04;
+                    rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_TASK_PERIOD_MS));
+                }
+            }
+#endif
+            break;
+
+        case RANGEFINDER_SRF10:
+#ifdef USE_RANGEFINDER_SRF10
+            if (srf10Detect(dev)) {
+                rangefinderHardware = RANGEFINDER_SRF10;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_SRF10_TASK_PERIOD_MS));
+            }
+#endif
+            break;
+
+            case RANGEFINDER_HCSR04I2C:
+#ifdef USE_RANGEFINDER_HCSR04_I2C
+            if (hcsr04i2c0Detect(dev)) {
+                rangefinderHardware = RANGEFINDER_HCSR04I2C;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS));
+            }
+#endif
+            break;
+
+            case RANGEFINDER_VL53L0X:
+#if defined(USE_RANGEFINDER_VL53L0X)
+            if (vl53l0xDetect(dev)) {
+                rangefinderHardware = RANGEFINDER_VL53L0X;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VL53L0X_TASK_PERIOD_MS));
+            }
+#endif
+            break;
+
+        case RANGEFINDER_UIB:
+#if defined(USE_RANGEFINDER_UIB)
+            if (uibRangefinderDetect(dev)) {
+                rangefinderHardware = RANGEFINDER_UIB;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_UIB_TASK_PERIOD_MS));
+            }
+#endif
+            break;
+
+        case RANGEFINDER_NONE:
+            rangefinderHardware = RANGEFINDER_NONE;
+            break;
+    }
+
+    if (rangefinderHardware == RANGEFINDER_NONE) {
+        sensorsClear(SENSOR_RANGEFINDER);
+        return false;
+    }
+
+    detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardware;
+    sensorsSet(SENSOR_RANGEFINDER);
+    return true;
+}
+
+void rangefinderResetDynamicThreshold(void)
+{
+    rangefinder.snrThresholdReached = false;
+    rangefinder.dynamicDistanceThreshold = 0;
+}
+
+bool rangefinderInit(void)
+{
+    if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
+        return false;
+    }
+
+    rangefinder.dev.init(&rangefinder.dev);
+    rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
+    rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
+    rangefinder.maxTiltCos = cos_approx(DECIDEGREES_TO_RADIANS(rangefinder.dev.detectionConeExtendedDeciDegrees / 2.0f));
+    rangefinder.lastValidResponseTimeMs = millis();
+    rangefinder.snr = 0;
+
+    rangefinderResetDynamicThreshold();
+
+    // XXX Interface to CF/BF legacy(?) altitude estimation code.
+    // XXX Will be gone once iNav's estimator is ported.
+    rangefinderMaxRangeCm = rangefinder.dev.maxRangeCm;
+    rangefinderMaxAltWithTiltCm = rangefinderMaxRangeCm * rangefinder.maxTiltCos;
+    rangefinderCfAltCm = rangefinder.dev.maxRangeCm / 2 ; // Complimentary Filter altitude
+
+    return true;
+}
+
+static int32_t applyMedianFilter(int32_t newReading)
+{
+    #define DISTANCE_SAMPLES_MEDIAN 5
+    static int32_t filterSamples[DISTANCE_SAMPLES_MEDIAN];
+    static int filterSampleIndex = 0;
+    static bool medianFilterReady = false;
+
+    if (newReading > RANGEFINDER_OUT_OF_RANGE) {// only accept samples that are in range
+        filterSamples[filterSampleIndex] = newReading;
+        ++filterSampleIndex;
+        if (filterSampleIndex == DISTANCE_SAMPLES_MEDIAN) {
+            filterSampleIndex = 0;
+            medianFilterReady = true;
+        }
+    }
+    return medianFilterReady ? quickMedianFilter5(filterSamples) : newReading;
+}
+
+static int16_t computePseudoSnr(int32_t newReading) {
+    #define SNR_SAMPLES 5
+    static int16_t snrSamples[SNR_SAMPLES];
+    static uint8_t snrSampleIndex = 0;
+    static int32_t previousReading = RANGEFINDER_OUT_OF_RANGE;
+    static bool snrReady = false;
+    int16_t pseudoSnr = 0;
+
+    snrSamples[snrSampleIndex] = constrain((int)(pow(newReading - previousReading, 2) / 10), 0, 6400);
+    ++snrSampleIndex;
+    if (snrSampleIndex == SNR_SAMPLES) {
+        snrSampleIndex = 0;
+        snrReady = true;
+    }
+
+    previousReading = newReading;
+
+    if (snrReady) {
+
+        for (uint8_t i = 0; i < SNR_SAMPLES; i++) {
+            pseudoSnr += snrSamples[i];
+        }
+
+        return constrain(pseudoSnr, 0, 32000);
+    } else {
+        return RANGEFINDER_OUT_OF_RANGE;
+    }
+}
+
+/*
+ * This is called periodically by the scheduler
+ */
+// XXX Returns timeDelta_t for iNav for pseudo-RT scheduling.
+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() {
+
+    /*
+     * Preconditions: raw and calculated altidude > 0
+     * SNR lower than threshold
+     */ 
+    if (
+        rangefinder.calculatedAltitude > 0 &&
+        rangefinder.rawAltitude > 0 &&
+        rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD
+    ) {
+
+        /*
+         * When critical altitude was determined, distance reported by rangefinder
+         * has to be lower than it to assume healthy readout
+         */
+        if (rangefinder.snrThresholdReached) {
+            return (rangefinder.rawAltitude < rangefinder.dynamicDistanceThreshold);
+        } else {
+            return true;
+        }
+
+    } else {
+        return false;
+    }
+
+}
+
+/**
+ * Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
+ */
+bool rangefinderProcess(float cosTiltAngle)
+{
+    if (rangefinder.dev.read) {
+        const int32_t distance = rangefinder.dev.read(&rangefinder.dev);
+
+        // If driver reported no new measurement - don't do anything
+        if (distance == RANGEFINDER_NO_NEW_DATA) {
+            return false;
+        }
+
+        if (distance >= 0) {
+            rangefinder.lastValidResponseTimeMs = millis();
+            rangefinder.rawAltitude = applyMedianFilter(distance);
+        }
+        else if (distance == RANGEFINDER_OUT_OF_RANGE) {
+            rangefinder.lastValidResponseTimeMs = millis();
+            rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
+        }
+        else {
+            // Invalid response / hardware failure
+            rangefinder.rawAltitude = RANGEFINDER_HARDWARE_FAILURE;
+        }
+
+        rangefinder.snr = computePseudoSnr(distance);
+
+        if (rangefinder.snrThresholdReached == false && rangefinder.rawAltitude > 0) {
+
+            if (rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD && rangefinder.dynamicDistanceThreshold < rangefinder.rawAltitude) {
+                rangefinder.dynamicDistanceThreshold = rangefinder.rawAltitude * RANGEFINDER_DYNAMIC_FACTOR / 100;
+            }
+
+            if (rangefinder.snr >= RANGEFINDER_DYNAMIC_THRESHOLD) {
+                rangefinder.snrThresholdReached = true;
+            }
+
+        }
+
+        DEBUG_SET(DEBUG_RANGEFINDER, 3, rangefinder.snr);
+
+        DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 0, rangefinder.rawAltitude);
+        DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 1, rangefinder.snrThresholdReached);
+        DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 2, rangefinder.dynamicDistanceThreshold);
+        DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 3, isSurfaceAltitudeValid());
+
+    }
+    else {
+        // Bad configuration
+        rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
+    }
+
+    /**
+    * 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 large, RANGEFINDER_OUT_OF_RANGE is returned.
+    */
+    if (cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0) {
+        rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
+    } else {
+        rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle;
+    }
+
+    DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
+    DEBUG_SET(DEBUG_RANGEFINDER, 2, rangefinder.calculatedAltitude);
+
+    return true;
+}
+
+/**
+ * Get the latest altitude that was computed, or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
+ * has never been called.
+ */
+int32_t rangefinderGetLatestAltitude(void)
+{
+    return rangefinder.calculatedAltitude;
+}
+
+int32_t rangefinderGetLatestRawAltitude(void) {
+    return rangefinder.rawAltitude;
+}
+
+bool rangefinderIsHealthy(void)
+{
+    return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;
+}
+#endif
+

src/main/sensors/rangefinder.h

@@ -0,0 +1,61 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "pg/pg.h"
+#include "drivers/rangefinder/rangefinder.h"
+
+typedef enum {
+    RANGEFINDER_NONE        = 0,
+    RANGEFINDER_HCSR04      = 1,
+    RANGEFINDER_SRF10       = 2,
+    RANGEFINDER_HCSR04I2C   = 3,
+    RANGEFINDER_VL53L0X     = 4,
+    RANGEFINDER_UIB         = 5,
+} rangefinderType_e;
+
+typedef struct rangefinderConfig_s {
+    uint8_t rangefinder_hardware;
+} rangefinderConfig_t;
+
+PG_DECLARE(rangefinderConfig_t, rangefinderConfig);
+
+typedef struct rangefinder_s {
+    rangefinderDev_t dev;
+    float maxTiltCos;
+    int32_t rawAltitude;
+    int32_t calculatedAltitude;
+    timeMs_t lastValidResponseTimeMs;
+    
+    bool snrThresholdReached;
+    int32_t dynamicDistanceThreshold;
+    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);
+bool rangefinderProcess(float cosTiltAngle);
+bool rangefinderIsHealthy(void);

src/main/sensors/sensors.h

@@ -22,9 +22,11 @@ typedef enum {
     SENSOR_INDEX_ACC,
     SENSOR_INDEX_BARO,
     SENSOR_INDEX_MAG,
+    SENSOR_INDEX_RANGEFINDER,
     SENSOR_INDEX_COUNT
 } sensorIndex_e;
 
+extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];
 extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
 
 typedef struct int16_flightDynamicsTrims_s {
@@ -48,6 +50,7 @@ typedef enum {
     SENSOR_BARO = 1 << 2,
     SENSOR_MAG = 1 << 3,
     SENSOR_SONAR = 1 << 4,
+    SENSOR_RANGEFINDER = 1 << 4,
     SENSOR_GPS = 1 << 5,
     SENSOR_GPSMAG = 1 << 6
 } sensors_e;

src/main/sensors/sonar.c

@@ -1,153 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <math.h>
-
-#include "platform.h"
-
-#ifdef USE_SONAR
-
-#include "build/build_config.h"
-
-#include "common/maths.h"
-#include "common/utils.h"
-
-#include "config/feature.h"
-#include "pg/pg.h"
-#include "pg/pg_ids.h"
-
-#include "drivers/io.h"
-
-#include "fc/runtime_config.h"
-
-#include "sensors/sensors.h"
-#include "sensors/battery.h"
-#include "sensors/sonar.h"
-
-// Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range.
-// Inclination is adjusted by imu
-
-int16_t sonarMaxRangeCm;
-int16_t sonarMaxAltWithTiltCm;
-int16_t sonarCfAltCm; // Complimentary Filter altitude
-STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees;
-float sonarMaxTiltCos;
-
-static int32_t calculatedAltitude;
-
-PG_REGISTER_WITH_RESET_TEMPLATE(sonarConfig_t, sonarConfig, PG_SONAR_CONFIG, 0);
-
-#ifndef SONAR_TRIGGER_PIN
-#define SONAR_TRIGGER_PIN NONE
-#endif
-#ifndef SONAR_ECHO_PIN
-#define SONAR_ECHO_PIN    NONE
-#endif
-
-PG_RESET_TEMPLATE(sonarConfig_t, sonarConfig,
-    .triggerTag = IO_TAG(SONAR_TRIGGER_PIN),
-    .echoTag = IO_TAG(SONAR_ECHO_PIN)
-);
-
-void sonarInit(const sonarConfig_t *sonarConfig)
-{
-    sonarRange_t sonarRange;
-
-    hcsr04_init(sonarConfig, &sonarRange);
-
-    sensorsSet(SENSOR_SONAR);
-    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(timeUs_t currentTimeUs)
-{
-    UNUSED(currentTimeUs);
-    hcsr04_start_reading();
-}
-
-/**
- * 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)
-{
-    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 large, SONAR_OUT_OF_RANGE is returned.
- */
-int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle)
-{
-    // calculate sonar altitude only if the ground is in the sonar cone
-    if (cosTiltAngle <= sonarMaxTiltCos)
-        calculatedAltitude = SONAR_OUT_OF_RANGE;
-    else
-        // altitude = distance * cos(tiltAngle), use approximation
-        calculatedAltitude = sonarDistance * cosTiltAngle;
-    return calculatedAltitude;
-}
-
-/**
- * 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)
-{
-    return calculatedAltitude;
-}
-
-#endif

src/main/target/CC3D/target.h

@@ -97,9 +97,10 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
-//#define USE_SONAR
+//#define USE_RANGEFINDER
-//#define SONAR_ECHO_PIN          PB0
+//#define USE_RANGEFINDER_HCSR04
-//#define SONAR_TRIGGER_PIN       PB5
+//#define RANGEFINDER_HCSR04_ECHO_PIN          PB0
+//#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB5
 
 #undef USE_MAG
 
@@ -107,7 +108,8 @@
 #define SKIP_CLI_COMMAND_HELP
 //#undef USE_SERVOS
 #undef USE_BARO
-#undef USE_SONAR
+#undef USE_RANGEFINDER
+#undef USE_RANGEFINDER_HCSR04
 #undef USE_SERIAL_4WAY_BLHELI_INTERFACE
 //#undef USE_SERIALRX_SPEKTRUM   // SRXL, DSM2 and DSMX protocol
 //#undef USE_SERIALRX_SBUS       // Frsky and Futaba receivers

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -52,11 +52,11 @@
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
-#include "sensors/sonar.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
+#include "sensors/rangefinder.h"
 
 #include "telemetry/telemetry.h"
 
@@ -173,7 +173,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXOSD, "OSD DISABLE SW;", 19 },
     { BOXTELEMETRY, "TELEMETRY;", 20 },
     { BOXGTUNE, "GTUNE;", 21 },
-    { BOXSONAR, "SONAR;", 22 },
+    { BOXRANGEFINDER, "RANGEFINDER;", 22 },
     { BOXSERVO1, "SERVO1;", 23 },
     { BOXSERVO2, "SERVO2;", 24 },
     { BOXSERVO3, "SERVO3;", 25 },
@@ -294,7 +294,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
 #else
             bstWrite16(0);
 #endif
-            bstWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
+            bstWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
             // BST the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
             // Requires new Multiwii protocol version to fix
             // It would be preferable to setting the enabled bits based on BOXINDEX.
@@ -319,7 +319,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
                     IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
                     IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
                     IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
-                    IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
+                    IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << BOXRANGEFINDER |
                     IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
                     IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
                     IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE;
@@ -628,7 +628,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
 static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/)
 {
     bstWrite8(BST_USB_DEVICE_INFO_FRAME);                        //Sub CPU Device Info FRAME
-    bstWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
+    bstWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
     bstWrite8(0x00);
     bstWrite8(0x00);
     bstWrite8(0x00);
@@ -841,13 +841,13 @@ bool writeFCModeToBST(void)
            IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << 3 |
            IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 |
-           IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << 6 |
+           IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << 6 |
            IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7;
 
     bstMasterStartBuffer(PUBLIC_ADDRESS);
     bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID);
     bstMasterWrite8(tmp);
-    bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
+    bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
 
     return bstMasterWrite(masterWriteData);
 }

src/main/target/EACHIF3/target.h

@@ -97,7 +97,8 @@
 
 #undef USE_GPS
 #undef USE_MAG
-#undef USE_SONAR
+#undef USE_RANGEFINDER
+#undef USE_RANGEFINDER_HCSR04
 #undef USE_SERVOS
 #undef BEEPER
 

src/main/target/ISHAPEDF3/target.h

@@ -49,9 +49,10 @@
 #define USE_FLASHFS
 #define USE_FLASH_M25P16
 
-#define USE_SONAR
+#define USE_RANGEFINDER
-#define SONAR_TRIGGER_PIN       PB0
+#define USE_RANGEFINDER_HCSR04
-#define SONAR_ECHO_PIN          PB1
+#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB0
+#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
 
 #define USE_UART1
 #define USE_UART2

src/main/target/MULTIFLITEPICO/target.h

@@ -57,9 +57,10 @@
 //#define USE_FLASHFS
 //#define USE_FLASH_M25P16
 
-#define USE_SONAR
+#define USE_RANGEFINDER
-#define SONAR_TRIGGER_PIN       PB0
+#define USE_RANGEFINDER_HCSR04
-#define SONAR_ECHO_PIN          PB1
+#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB0
+#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
 
 #define USE_UART1
 #define USE_UART2

src/main/target/NAZE/target.h

@@ -110,11 +110,12 @@
 #define MAG_HMC5883_ALIGN       CW180_DEG
 */
 
-//#define USE_SONAR
+//#define USE_RANGEFINDER
-//#define SONAR_TRIGGER_PIN       PB0
+//#define USE_RANGEFINDER_HCSR04
-//#define SONAR_ECHO_PIN          PB1
+//#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB0
-//#define SONAR_TRIGGER_PIN_PWM   PB8
+//#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
-//#define SONAR_ECHO_PIN_PWM      PB9
+//#define RANGEFINDER_HCSR04_TRIGGER_PIN_PWM   PB8
+//#define RANGEFINDER_HCSR04_ECHO_PIN_PWM      PB9
 
 #define USE_UART1
 #define USE_UART2

src/main/target/OMNIBUS/target.h

@@ -59,9 +59,10 @@
 #define USE_BARO_BMP280
 #define USE_BARO_SPI_BMP280
 
-//#define USE_SONAR
+//#define USE_RANGEFINDER
-//#define SONAR_ECHO_PIN          PB1
+//#define USE_RANGEFINDER_HCSR04
-//#define SONAR_TRIGGER_PIN       PB0
+//#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
+//#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB0
 
 #define USB_DETECT_PIN          PB5
 

src/main/target/OMNIBUSF4/target.h

@@ -243,7 +243,10 @@
 
 #define USE_TRANSPONDER
 
-#define USE_SONAR
+#define USE_RANGEFINDER
+#define USE_RANGEFINDER_HCSR04
+#define RANGEFINDER_HCSR04_TRIGGER_PIN     PA1
+#define RANGEFINDER_HCSR04_ECHO_PIN        PA8
 
 #define DEFAULT_RX_FEATURE      FEATURE_RX_SERIAL
 

src/main/target/RCEXPLORERF3/target.h

@@ -52,9 +52,10 @@
 
 #define MAG_AK8975_ALIGN CW180_DEG
 
-#define USE_SONAR
+#define USE_RANGEFINDER
-#define SONAR_TRIGGER_PIN           PA6   // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
+#define USE_RANGEFINDER_HCSR04
-#define SONAR_ECHO_PIN              PB1   // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
+#define RANGEFINDER_HCSR04_TRIGGER_PIN           PA6   // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
+#define RANGEFINDER_HCSR04_ECHO_PIN              PB1   // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
 
 #define USE_VCP
 #define USE_UART1

src/main/target/SPARKY/target.h

@@ -90,9 +90,10 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
-//#define USE_SONAR
+//#define USE_RANGEFINDER
-//#define SONAR_ECHO_PIN          PB1
+//#define USE_RANGEFINDER_HCSR04
-//#define SONAR_TRIGGER_PIN       PA2
+//#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
+//#define RANGEFINDER_HCSR04_TRIGGER_PIN       PA2
 
 // available IO pins (from schematics)
 //#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15))

src/main/target/SPRACINGF3/target.h

@@ -89,9 +89,10 @@
 #endif
 
 #if defined(FLIP32F3OSD)
-#define USE_SONAR
+#define USE_RANGEFINDER
-#define SONAR_TRIGGER_PIN       PB0
+#define USE_RANGEFINDER_HCSR04
-#define SONAR_ECHO_PIN          PB1
+#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB0
+#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
 
 #elif defined(RMDO)
 #undef USE_GPS
@@ -102,9 +103,10 @@
 
 #else //SPRACINGF3
 
-#define USE_SONAR
+#define USE_RANGEFINDER
-#define SONAR_TRIGGER_PIN       PB0
+#define USE_RANGEFINDER_HCSR04
-#define SONAR_ECHO_PIN          PB1
+#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB0
+#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
 
 #define USE_BARO_MS5611
 #define USE_BARO_BMP085
@@ -137,7 +139,7 @@
 #define SOFTSERIAL2_RX_PIN      PB0 // PWM 7
 #define SOFTSERIAL2_TX_PIN      PB1 // PWM 8
 
-#define SONAR_SOFTSERIAL2_EXCLUSIVE
+#define RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE
 #endif
 
 #define USE_ESCSERIAL

src/main/target/SPRACINGF3EVO/target.h

@@ -82,7 +82,8 @@
 
 #define MAG_AK8963_ALIGN CW90_DEG_FLIP
 
-//#define USE_SONAR
+//#define USE_RANGEFINDER
+//#define USE_RANGEFINDER_HCSR04
 
 #define USE_VCP
 #define USE_UART1

src/main/target/SPRACINGF3MINI/target.h

@@ -74,9 +74,10 @@
 #define MAG_AK8975_ALIGN        CW90_DEG_FLIP
 #endif
 
-//#define USE_SONAR
+//#define USE_RANGEFINDER
-//#define SONAR_ECHO_PIN          PB1
+//#define USE_RANGEFINDER_HCSR04
-//#define SONAR_TRIGGER_PIN       PB0
+//#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
+//#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB0
 
 #define USE_BRUSHED_ESC_AUTODETECT
 
@@ -122,7 +123,7 @@
 #define SOFTSERIAL1_TX_PIN      PA1 // PA1 / PAD4
 #endif
 
-#define SONAR_SOFTSERIAL1_EXCLUSIVE
+#define RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE
 
 #define USE_SPI
 

src/main/target/STM32F3DISCOVERY/target.h

@@ -194,9 +194,10 @@
 
 #define USE_ESC_SENSOR
 
-#define USE_SONAR
+#define USE_RANGEFINDER
-#define SONAR_TRIGGER_PIN       PB0
+#define USE_RANGEFINDER_HCSR04
-#define SONAR_ECHO_PIN          PB1
+#define RANGEFINDER_HCSR04_TRIGGER_PIN       PB0
+#define RANGEFINDER_HCSR04_ECHO_PIN          PB1
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 

src/main/target/X_RACERSPI/target.h

@@ -72,7 +72,7 @@
 
 #define SOFTSERIAL1_RX_PIN      PB0  // PWM 5
 #define SOFTSERIAL1_TX_PIN      PB1  // PWM 6
-#define SONAR_SOFTSERIAL1_EXCLUSIVE
+#define RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE
 
 #define USE_I2C
 #define USE_I2C_DEVICE_1

src/main/telemetry/frsky.c

@@ -334,7 +334,7 @@ static void sendGPSLatLong(void)
 #endif
 #endif
 
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
 /*
  * Send vertical speed for opentx. ID_VERT_SPEED
  * Unit is cm/s
@@ -545,8 +545,8 @@ void handleFrSkyTelemetry(timeUs_t currentTimeUs)
         sendAccel();
     }
 
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-    if (sensors(SENSOR_BARO | SENSOR_SONAR)) {
+    if (sensors(SENSOR_BARO | SENSOR_RANGEFINDER)) {
         // Sent every 125ms
         sendVario();
 

src/main/telemetry/ltm.c

@@ -142,8 +142,8 @@ static void ltm_gframe(void)
     ltm_serialise_32(gpsSol.llh.lon);
     ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
 
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-    ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100;
+    ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100;
 #else
     ltm_alt = gpsSol.llh.alt * 100;
 #endif

src/main/telemetry/mavlink.c

@@ -329,8 +329,8 @@ void mavlinkSendPosition(void)
         // alt Altitude in 1E3 meters (millimeters) above MSL
         gpsSol.llh.alt * 1000,
         // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-        (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000,
+        (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000,
 #else
         gpsSol.llh.alt * 1000,
 #endif
@@ -396,8 +396,8 @@ void mavlinkSendHUDAndHeartbeat(void)
 #endif
 
     // select best source for altitude
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-    if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
+    if (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) {
         // Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
         mavAltitude = getEstimatedAltitude() / 100.0;
     }
@@ -478,7 +478,7 @@ void mavlinkSendHUDAndHeartbeat(void)
         mavCustomMode = 0;      //Stabilize
         mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
     }
-    if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE))
+    if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE))
         mavCustomMode = 2;      //Alt Hold
     if (FLIGHT_MODE(GPS_HOME_MODE))
         mavCustomMode = 6;      //Return to Launch

src/main/telemetry/smartport.c

@@ -464,7 +464,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                     tmpi += 100;
                 if (FLIGHT_MODE(BARO_MODE))
                     tmpi += 200;
-                if (FLIGHT_MODE(SONAR_MODE))
+                if (FLIGHT_MODE(RANGEFINDER_MODE))
                     tmpi += 400;
 
                 if (FLIGHT_MODE(GPS_HOLD_MODE))