1 sensor using RTOS Timer, another 2 over RTOS Semaphore

Dependents:   Autonomous_quadcopter

Fork of HCSR04 by Antoniolinux B.

Revision:
1:53657de3246f
Parent:
0:86b2086be101
Child:
2:8801a3da8693
--- a/hcsr04.cpp	Mon Apr 14 08:23:09 2014 +0000
+++ b/hcsr04.cpp	Thu Oct 26 15:54:16 2017 +0000
@@ -1,29 +1,42 @@
 #include "hcsr04.h"
 #include "mbed.h"
+#include "rtos.h"
 /*
 *HCSR04.cpp
 */
-HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) {}
- long HCSR04::echo_duration() {
+HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) {
+    thread = new Thread(threadWorker, this);
+}
+
+void HCSR04::threadWorker(void const *p)
+{
+    HCSR04* self = (HCSR04*)p;
+    
+    while(1){
         
-    timer.reset();  //reset timer
-    trig=0;   // trigger low 
-    wait_us(2); //  wait 
-    trig=1;   //  trigger high
-    wait_us(10);
-    trig=0;  // trigger low
-         while(!echo); // start pulseIN
-      timer.start();
-     while(echo);
-      timer.stop();
-     return timer.read_us(); 
- 
+        self->trig=0;   // trigger low 
+        wait_us(2); //  wait 
+        self->trig=1;   //  trigger high
+        wait_us(10);
+        self->trig=0;  // trigger low
+        while(!self->echo); // start pulseIN
+        self->timer.reset();  //reset timer
+        self->timer.start();
+        while(self->echo);
+        self->timer.stop();
+        self->distan = self->timer.read_us() / 58;
+        
+        Thread::wait(50);
+    }
+}
+
+uint16_t HCSR04::getDistan(){
+    return distan;    
 }
  
 //return distance in cm 
-long HCSR04::distance(){
-    duration = echo_duration();
-  distance_cm = (duration/2)/29.1  ;
-        return distance_cm;
+uint16_t HCSR04::getDistance(){
+    distance_cm = distan/58;
+    return distance_cm;
 
 }
\ No newline at end of file