Diff: RangeFinder.cpp

Revision:

4:fe63b514d4ef

Parent:

2:89d7dd44ecfd

--- a/RangeFinder.cpp	Fri Jun 21 08:57:37 2019 +0000
+++ b/RangeFinder.cpp	Fri Jul 12 15:19:53 2019 +0000
@@ -22,16 +22,71 @@
  
  #include "RangeFinder.h"
 
-RangeFinder::RangeFinder(PinName pin, int pulsetime, float scale, float offset, int time):
-    pio(pin), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+// Init with 1 Sensor:
+RangeFinder::RangeFinder(PinName pin0, int pulsetime, float scale, float offset, int time):
+    pio0(pin0), pio1(NC), pio2(NC), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+    n_sensors = 1;
+    this->old_val[0] = 0.0;
+    current_sensor = 0;
+}
+
+// Init with 2 Sensors:
+RangeFinder::RangeFinder(PinName pin0, PinName pin1,int pulsetime, float scale, float offset, int time):
+    pio0(pin0), pio1(pin1), pio2(NC), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+    n_sensors = 2;
+    for(uint8_t k=0;k<n_sensors;k++)
+        this->old_val[k] = 0.0;
+    current_sensor = 0;
+    
+}
+// Init with 3 Sensors:
+RangeFinder::RangeFinder(PinName pin0,PinName pin1, PinName pin2, int pulsetime, float scale, float offset, int time):
+    pio0(pin0), pio1(pin1), pio2(pin2), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+    n_sensors = 3;
+    for(uint8_t k=0;k<n_sensors;k++)
+        this->old_val[k] = 0.0;
+    current_sensor = 0;
+}
+// Init with 4 Sensors:
+RangeFinder::RangeFinder(PinName pin0, PinName pin1, PinName pin2,PinName pin3, int pulsetime, float scale, float offset, int time):
+    pio0(pin0), pio1(pin1), pio2(pin2), pio3(pin3), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+    n_sensors = 4;
+    for(uint8_t k=0;k<n_sensors;k++)
+        this->old_val[k] = 0.0;
+    current_sensor = 0;
 }
 
 RangeFinder::~RangeFinder() {
 }
-
-float RangeFinder::read_m()  {
-    pio.write_us(1, pulsetime);
-    float t = (float) pio.read_high_us(timeout);
+void RangeFinder::read_and_filter(uint8_t i)  {
+val[i] = this->read_m(i);
+if(val[i] < 0)
+    val[i] = old_val[i];
+else{
+    old_val[i] = val[i];
+    }
+}
+// -----------------------------------------------------------------------------
+float RangeFinder::read_m(uint8_t i)  {
+    float t;
+    switch(i){
+        case 0:
+            pio0.write_us(1, pulsetime);
+            t = (float) pio0.read_high_us(timeout);
+            break;
+        case 1:
+            pio1.write_us(1, pulsetime);
+            t = (float) pio1.read_high_us(timeout);
+            break;
+        case 2:
+            pio2.write_us(1, pulsetime);
+            t = (float) pio2.read_high_us(timeout);
+            break;
+        case 3:
+            pio3.write_us(1, pulsetime);
+            t = (float) pio3.read_high_us(timeout);
+            break;
+        }
     if (t == -1.0)   {
         return -1.0;
     }