Diff: hcsr04.cpp

Revision:

7:dcf0aa89bcd7

Parent:

6:7f98f258e8ef

Child:

8:0a10bacaf501

--- a/hcsr04.cpp	Sun Feb 18 17:10:25 2018 +0000
+++ b/hcsr04.cpp	Sun Feb 25 18:31:44 2018 +0000
@@ -5,55 +5,96 @@
 /*
 *HCSR04.cpp
 */
-HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) {
-    //thread = new Thread(threadWorker, this);
-    threadUpdateTimer = new RtosTimer(threadWorker, osTimerPeriodic, this);
+HCSR04::HCSR04(PinName t1, PinName e1, PinName t2, PinName e2) : trig1(t1), echo1(e1), trig2(t2), echo2(e2) {
+    
     int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
-    threadUpdateTimer->start(rate);
+    
+    limit = (rate * 1000 - rate * 200);
+    
+    threadUpdateTimer1 = new RtosTimer(threadWorker1, osTimerPeriodic, this);
+    threadUpdateTimer1->start(rate);
+    
+    //threadUpdateTimer2 = new RtosTimer(threadWorker2, osTimerPeriodic, this);
+    //threadUpdateTimer2->start(rate);
 }
 
-void HCSR04::threadWorker(void const *p)
+void HCSR04::threadWorker1(void const *p)
 {
     HCSR04* self = (HCSR04*)p;
     
-        self->trig=1;   //  trigger high
+        //printf("start\n\r");
+        self->trig1=1;   //  trigger high
+        //printf("trig\n\r");
         wait_us(10);
-        self->trig=0;  // trigger low
+        //printf("wait\n\r");
+        self->trig1=0;  // trigger low
         // start pulseIN
-        self->timer.reset();
-        self->timer.start();
-        while(!self->echo){
-            if(self->timer.read_us() > 4500){
+        self->timer1.reset();
+        self->timer1.start();
+        while(!self->echo1){
+            if(self->timer1.read_us() > self->limit){
                 //printf("moc1\n\r");
                 // if no response from distance sensor -> let the quadcopter fall
-                self->distan = 500;
-                return;
+                break;
             }    
         }
-        if(self->timer.read_us() < 4500){
-            self->timer.stop();
-            self->timer.reset();  //reset timer
-            self->timer.start();
-            while(self->echo){
-                if(self->timer.read_us() > 4500){
+        if(self->timer1.read_us() < self->limit){
+            self->timer1.stop();
+            self->timer1.reset();  //reset timer
+            self->timer1.start();
+            while(self->echo1){
+                if(self->timer1.read_us() > self->limit){
                     //printf("moc2\n\r");
                     // if no response from distance sensor -> let the quadcopter fall
-                    self->distan = 500;
-                    return;
+                    break;
                 }
             }
-            self->timer.stop();
-            self->distan = self->timer.read_us() / 58;
+            self->timer1.stop();
+            self->distan1 = self->timer1.read_us() / 58;
+        }
+        //printf("end\n\r");
+}
+
+void HCSR04::threadWorker2(void const *p)
+{
+    HCSR04* self = (HCSR04*)p;
+    
+        self->trig2=1;   //  trigger high
+        wait_us(10);
+        self->trig2=0;  // trigger low
+        // start pulseIN
+        self->timer2.reset();
+        self->timer2.start();
+        while(!self->echo2){
+            if(self->timer2.read_us() > self->limit){
+                //printf("moc1\n\r");
+                // if no response from distance sensor -> let the quadcopter fall
+                //self->distan2 = 500;
+                break;
+            }    
+        }
+        if(self->timer2.read_us() < self->limit){
+            self->timer2.stop();
+            self->timer2.reset();  //reset timer
+            self->timer2.start();
+            while(self->echo2){
+                if(self->timer2.read_us() > self->limit){
+                    //printf("moc2\n\r");
+                    // if no response from distance sensor -> let the quadcopter fall
+                    //self->distan2 = 500;
+                    break;
+                }
+            }
+            self->timer2.stop();
+            self->distan2 = self->timer2.read_us() / 58;
         }
 }
 
-uint16_t HCSR04::getDistan(){
-    return distan;    
+uint16_t HCSR04::getDistan1(){
+    return distan1;    
+}
+
+uint16_t HCSR04::getDistan2(){
+    return distan2;    
 }
  
-//return distance in cm 
-uint16_t HCSR04::getDistance(){
-    distance_cm = distan/58;
-    return distance_cm;
-
-}
\ No newline at end of file