1 sensor using RTOS Timer, another 2 over RTOS Semaphore
Dependents: Autonomous_quadcopter
Fork of HCSR04 by
Diff: hcsr04.cpp
- Revision:
- 7:dcf0aa89bcd7
- Parent:
- 6:7f98f258e8ef
- Child:
- 8:0a10bacaf501
diff -r 7f98f258e8ef -r dcf0aa89bcd7 hcsr04.cpp --- 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