1 sensor using RTOS Timer, another 2 over RTOS Semaphore
Dependents: Autonomous_quadcopter
Fork of HCSR04 by
hcsr04.cpp
- Committer:
- edy05
- Date:
- 2018-02-25
- Revision:
- 7:dcf0aa89bcd7
- Parent:
- 6:7f98f258e8ef
- Child:
- 8:0a10bacaf501
File content as of revision 7:dcf0aa89bcd7:
#include "hcsr04.h" #include "definitions.h" #include "mbed.h" #include "rtos.h" /* *HCSR04.cpp */ 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; 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::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->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 break; } } 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 break; } } 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::getDistan1(){ return distan1; } uint16_t HCSR04::getDistan2(){ return distan2; }