1 sensor using RTOS Timer, another 2 over RTOS Semaphore
Dependents: Autonomous_quadcopter
Fork of HCSR04 by
Diff: hcsr04.cpp
- Revision:
- 3:81512ca9a13c
- Parent:
- 2:8801a3da8693
- Child:
- 4:34023f76fc4f
- Child:
- 5:02948ae78be7
diff -r 8801a3da8693 -r 81512ca9a13c hcsr04.cpp --- a/hcsr04.cpp Fri Dec 01 11:10:54 2017 +0000 +++ b/hcsr04.cpp Sat Jan 20 11:44:21 2018 +0000 @@ -5,29 +5,40 @@ *HCSR04.cpp */ HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) { - thread = new Thread(threadWorker, this); + //thread = new Thread(threadWorker, this); + threadUpdateTimer = new RtosTimer(threadWorker, osTimerPeriodic, this); + threadUpdateTimer->start(5); } void HCSR04::threadWorker(void const *p) { HCSR04* self = (HCSR04*)p; - while(1){ - - 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 + // start pulseIN + self->timer.reset(); self->timer.start(); - while(self->echo); - self->timer.stop(); - self->distan = self->timer.read_us() / 58; - - Thread::wait(2); - } + while(!self->echo){ + if(self->timer.read_us() > 4500){ + //printf("moc1\n\r"); + 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){ + //printf("moc2\n\r"); + break; + } + } + self->timer.stop(); + self->distan = self->timer.read_us() / 58; + } } uint16_t HCSR04::getDistan(){