From c8b26b45c7e6b2470b5f6dd49eebc0bf9a8da32a Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 6 Jun 2018 15:16:11 +0200
Subject: [PATCH] Cleanup of unused functions

---
 src/main/fc/fc_core.c          |  8 ----
 src/main/sensors/rangefinder.c | 80 ----------------------------------
 src/main/sensors/rangefinder.h |  5 ---
 3 files changed, 93 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 08ebde6f717..c873d620370 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -401,14 +401,6 @@ void tryArm(void)
 #endif
         statsOnArm();
 
-#ifdef USE_RANGEFINDER
-        /*
-         * Since each arm can happen over different surface type, we have to reset
-         * previously computed max. dynamic range threshold
-         */ 
-        rangefinderResetDynamicThreshold();
-#endif
-
         return;
     }
 
diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c
index dd68a149970..e05b95c5840 100644
--- a/src/main/sensors/rangefinder.c
+++ b/src/main/sensors/rangefinder.c
@@ -169,12 +169,6 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
     return true;
 }
 
-void rangefinderResetDynamicThreshold(void)
-{
-    rangefinder.snrThresholdReached = false;
-    rangefinder.dynamicDistanceThreshold = 0;
-}
-
 bool rangefinderInit(void)
 {
     if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
@@ -186,9 +180,6 @@ bool rangefinderInit(void)
     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();
 
     return true;
 }
@@ -211,35 +202,6 @@ static int32_t applyMedianFilter(int32_t newReading)
     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
  */
@@ -252,34 +214,6 @@ timeDelta_t rangefinderUpdate(void)
     return rangefinder.dev.delayMs * 1000;  // to microseconds
 }
 
-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.
  */
@@ -309,20 +243,6 @@ bool rangefinderProcess(float cosTiltAngle)
             // 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;
-            }
-
-        }
     }
     else {
         // Bad configuration
diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h
index 70e9ed6af0e..cfb67e71d57 100644
--- a/src/main/sensors/rangefinder.h
+++ b/src/main/sensors/rangefinder.h
@@ -44,17 +44,12 @@ typedef struct rangefinder_s {
     int32_t rawAltitude;
     int32_t calculatedAltitude;
     timeMs_t lastValidResponseTimeMs;
-    
-    bool snrThresholdReached;
-    int32_t dynamicDistanceThreshold;
-    int16_t snr;
 } rangefinder_t;
 
 extern rangefinder_t rangefinder;
 
 const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void);
 
-void rangefinderResetDynamicThreshold(void);
 bool rangefinderInit(void);
 
 int32_t rangefinderGetLatestAltitude(void);