From c63b4d01f0ee712b4be2da29cce7c54186e6a764 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sun, 9 Apr 2023 20:33:45 +0200
Subject: [PATCH] add filter

---
 platformio.ini |  3 +-
 src/main.cpp   | 75 ++++++++++++++++++++++++++++++++++++++++++++------
 2 files changed, 69 insertions(+), 9 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index 02ecd40..0a8c3e0 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -17,4 +17,5 @@ monitor_speed = 115200
 
 lib_deps =
     https://github.com/milesburton/Arduino-Temperature-Control-Library/
-    d03n3rfr1tz3/HC-SR04@^1.1.2
\ No newline at end of file
+    d03n3rfr1tz3/HC-SR04@^1.1.2
+    https://github.com/emilv/ArduinoSort/
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index af24181..4b6900b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,5 +1,5 @@
 #include <Arduino.h>
-
+#include <ArduinoSort.h>
 
 // ######## EC
 #define EC_PIN_ADC 4
@@ -48,6 +48,7 @@ float tempCmean_air[TEMPMEAN_SIZE];
 #include <HCSR04.h>
 #define HCSR04_PIN_ECHO 17
 #define HCSR04_PIN_TRIGGER 16
+#define HCSR04_TIMEOUT 5000 //default is 100000 (uS)
 #define READINTERVAL_HCSR04 100
 
 #define WATERLEVELMEAN_SIZE 32
@@ -76,8 +77,11 @@ float getMean(uint16_t *parray,uint16_t psize);
 float getMeanf(float *parray,uint16_t psize);
 uint16_t getMin(uint16_t *parray, uint16_t psize);
 uint16_t getMax(uint16_t *parray, uint16_t psize);
+float getMaxf(float *parray,uint16_t psize);
+float getMinf(float *parray, uint16_t psize);
 bool isValueArrayOK(uint16_t *parray,uint16_t psize, uint16_t pcheck);
 bool isValueArrayOKf(float *parray,uint16_t psize, float pcheck);
+float getFilteredf(float *parray,uint16_t psize, uint16_t pcutOff);
 
 
 void printAddress(DeviceAddress deviceAddress);
@@ -96,7 +100,12 @@ void setup() {
   ledcAttachPin(EC_PIN_FREQ, EC_PWM_CH);
   ledcWrite(EC_PWM_CH, 127);
 
-  HCSR04.begin(HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO);
+  //HCSR04.begin(HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO);
+  HCSR04.begin(HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO,HCSR04_TIMEOUT, HCSR04.eUltraSonicUnlock_t::unlockSkip);
+  for (uint16_t i=0;i<WATERLEVELMEAN_SIZE;i++) {
+    waterlevelMean[i]=-1; //-1 is also timeout value
+    
+  }
 
   //initialize mean array
   for (uint16_t i=0;i<TEMPMEAN_SIZE;i++) {
@@ -213,7 +222,6 @@ void loop() {
       temperature=getMeanf(tempCmean_air,TEMPMEAN_SIZE);
     }
     
-    
     double* distances = HCSR04.measureDistanceMm(temperature);
 
     waterlevelMean[waterlevelMean_pos]=distances[0];
@@ -236,9 +244,14 @@ void loop() {
   if (loopmillis>last_print+500) { 
     last_print=loopmillis;
 
-    Serial.print("EC=");
-    Serial.print(getMean(ec_array,EC_ARRAY_SIZE),3);
-    Serial.print("\t spread="); Serial.print(getMax(ec_array,EC_ARRAY_SIZE) - getMin(ec_array,EC_ARRAY_SIZE)); 
+    if (isValueArrayOK(ec_array,EC_ARRAY_SIZE,0))
+    {
+      Serial.print("EC=");
+      Serial.print(getMean(ec_array,EC_ARRAY_SIZE),3);
+      Serial.print(" count (+- ");  Serial.print( (getMax(ec_array,EC_ARRAY_SIZE) - getMin(ec_array,EC_ARRAY_SIZE))/2.0);  Serial.print(")");
+    }else{
+      Serial.print("Waiting for EC");
+    }
 
     if (isValueArrayOKf(tempCmean_reservoir,TEMPMEAN_SIZE,DEVICE_DISCONNECTED_C)){
       Serial.print("\t Treservoir="); Serial.print(getMeanf(tempCmean_reservoir,TEMPMEAN_SIZE)); Serial.print("\t Tair="); Serial.print(getMeanf(tempCmean_air,TEMPMEAN_SIZE));
@@ -247,8 +260,16 @@ void loop() {
     }
 
     
-    if (isValueArrayOKf(waterlevelMean,WATERLEVELMEAN_SIZE,0)){
-      Serial.print("\t Dist="); Serial.print(getMeanf(waterlevelMean,WATERLEVELMEAN_SIZE));  Serial.print("mm");
+    if (isValueArrayOKf(waterlevelMean,WATERLEVELMEAN_SIZE,-1.0)){
+      float _max=getMaxf(waterlevelMean,WATERLEVELMEAN_SIZE);
+      float _min=getMinf(waterlevelMean,WATERLEVELMEAN_SIZE);
+
+      float _filteredWaterlevel=getFilteredf(waterlevelMean,WATERLEVELMEAN_SIZE,8);
+      float _meanWaterlevel=getMeanf(waterlevelMean,WATERLEVELMEAN_SIZE);
+      
+
+      Serial.print("\t Dist="); Serial.print(_filteredWaterlevel); Serial.print("mm"); Serial.print("(+- "); Serial.print((_max-_min)/2.0); Serial.print(")"); Serial.print(" [mean="); Serial.print(_meanWaterlevel); Serial.print("]");
+      
     }else{
       Serial.print("\t waiting for distance");
     }
@@ -323,6 +344,28 @@ uint16_t getMax(uint16_t *parray,uint16_t psize) {
   return max;
 }
 
+float getMinf(float *parray, uint16_t psize) {
+  float min=65535;
+  for (uint16_t i=0;i<psize;i++) {
+    if (parray[i]<min) {
+      min=parray[i];
+    }
+  }
+
+  return min;
+}
+
+float getMaxf(float *parray,uint16_t psize) {
+  float max=0;
+  for (uint16_t i=0;i<psize;i++) {
+    if (parray[i]>max) {
+      max=parray[i];
+    }
+  }
+
+  return max;
+}
+
 
 
 
@@ -340,4 +383,20 @@ void printAddress(DeviceAddress deviceAddress)
 void IRAM_ATTR isr_flow() {
 	flow_counter++;
   flow_counter_sum++;
+}
+
+float getFilteredf(float *parray,uint16_t psize, uint16_t pcutOff) {
+  //cuts off lowest and highest pcutOff values from array, then returns the mean of the psize-2*pcutOff center values.
+  //pcutOff < psize/2
+
+  float _copy[psize];
+  std::copy(parray,parray + psize, _copy);
+  sortArray(_copy,psize);
+
+  double mean=0;
+  for (uint16_t i=pcutOff;i<psize-pcutOff;i++) {
+    mean+=_copy[i];
+  }
+
+  return mean/(psize-2*pcutOff);
 }
\ No newline at end of file