Diff: Sources/DistanceSensors.cpp

Revision:

17:4e1be70bdedb

Parent:

4:67d7177c213f

--- a/Sources/DistanceSensors.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/DistanceSensors.cpp	Thu May 18 14:27:09 2017 +0000
@@ -38,7 +38,7 @@
 {
     
     float Usensor;
-  
+
     if (number < 6){
         *bit0 = number & 1; // Set the first bit of the Sensors MUX
         *bit1 = number & 2; // Set the second bit of the Sensors MUX
@@ -61,8 +61,15 @@
     }
     
     //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
-    float Distance=-0.38f*sqrt(Usensor)+0.38f;
+    Usensor *= 3.3f;
+    float Distance= 5.906*Usensor*Usensor - 30.831*Usensor + 47.628;
+    Distance /= 100;
+    
+    static float distance_filtered = 0.0f;
+    distance_filtered = 0.55f * distance_filtered + 0.45f * Distance;
+    
     return Distance;
+    return distance_filtered;
 }
 
 DistanceSensors::operator float()