1 sensor using RTOS Timer, another 2 over RTOS Semaphore
Dependents: Autonomous_quadcopter
Fork of HCSR04 by
hcsr04.cpp
- Committer:
- edy05
- Date:
- 2018-05-22
- Revision:
- 13:e9b99635aa2d
- Parent:
- 12:6f6469b2f016
File content as of revision 13:e9b99635aa2d:
#include "hcsr04.h" #include "definitions.h" #include "mbed.h" #include "rtos.h" /* *HCSR04.cpp */ HCSR04::HCSR04(PinName t1, PinName e1) : trig1(t1), echo1(e1){ int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; //ms limit = (rate * 1000 - rate * 200); threadUpdateTimer1 = new RtosTimer(threadWorker1, osTimerPeriodic, this); threadUpdateTimer1->start(rate); id = Thread::gettid(); printf("down sonic gettid 0x%08X \n\r", id); //threadUpdateTimer2 = new RtosTimer(threadWorker2, osTimerPeriodic, this); //threadUpdateTimer2->start(rate); } void HCSR04::threadWorker1(void const *p) { HCSR04* self = (HCSR04*)p; //printf("start\n\r"); self->trig1=1; // trigger high //printf("trig\n\r"); wait_us(10); //printf("wait\n\r"); self->trig1=0; // trigger low // start pulseIN self->timerLimit.reset(); self->timerLimit.start(); while(!self->echo1){ if(self->timerLimit.read_us() > self->limit){ //printf("moc1\n\r"); break; } } if(self->timerLimit.read_us() < self->limit){ //self->timer1.stop(); self->timer1.reset(); //reset timer self->timer1.start(); while(self->echo1){ if(self->timerLimit.read_us() > self->limit){ //printf("moc2\n\r"); break; } } self->timer1.stop(); self->distan1 = self->timer1.read_us() / 58; } //printf("end\n\r"); } float HCSR04::getDistan1(){ return distan1; }