1 sensor using RTOS Timer, another 2 over RTOS Semaphore
Dependents: Autonomous_quadcopter
Fork of HCSR04 by
hcsr04.cpp@7:dcf0aa89bcd7, 2018-02-25 (annotated)
- Committer:
- edy05
- Date:
- Sun Feb 25 18:31:44 2018 +0000
- Revision:
- 7:dcf0aa89bcd7
- Parent:
- 6:7f98f258e8ef
- Child:
- 8:0a10bacaf501
some update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
antoniolinux | 0:86b2086be101 | 1 | #include "hcsr04.h" |
edy05 | 5:02948ae78be7 | 2 | #include "definitions.h" |
antoniolinux | 0:86b2086be101 | 3 | #include "mbed.h" |
edy05 | 1:53657de3246f | 4 | #include "rtos.h" |
antoniolinux | 0:86b2086be101 | 5 | /* |
antoniolinux | 0:86b2086be101 | 6 | *HCSR04.cpp |
antoniolinux | 0:86b2086be101 | 7 | */ |
edy05 | 7:dcf0aa89bcd7 | 8 | HCSR04::HCSR04(PinName t1, PinName e1, PinName t2, PinName e2) : trig1(t1), echo1(e1), trig2(t2), echo2(e2) { |
edy05 | 7:dcf0aa89bcd7 | 9 | |
edy05 | 5:02948ae78be7 | 10 | int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; |
edy05 | 7:dcf0aa89bcd7 | 11 | |
edy05 | 7:dcf0aa89bcd7 | 12 | limit = (rate * 1000 - rate * 200); |
edy05 | 7:dcf0aa89bcd7 | 13 | |
edy05 | 7:dcf0aa89bcd7 | 14 | threadUpdateTimer1 = new RtosTimer(threadWorker1, osTimerPeriodic, this); |
edy05 | 7:dcf0aa89bcd7 | 15 | threadUpdateTimer1->start(rate); |
edy05 | 7:dcf0aa89bcd7 | 16 | |
edy05 | 7:dcf0aa89bcd7 | 17 | //threadUpdateTimer2 = new RtosTimer(threadWorker2, osTimerPeriodic, this); |
edy05 | 7:dcf0aa89bcd7 | 18 | //threadUpdateTimer2->start(rate); |
edy05 | 1:53657de3246f | 19 | } |
edy05 | 1:53657de3246f | 20 | |
edy05 | 7:dcf0aa89bcd7 | 21 | void HCSR04::threadWorker1(void const *p) |
edy05 | 1:53657de3246f | 22 | { |
edy05 | 1:53657de3246f | 23 | HCSR04* self = (HCSR04*)p; |
edy05 | 1:53657de3246f | 24 | |
edy05 | 7:dcf0aa89bcd7 | 25 | //printf("start\n\r"); |
edy05 | 7:dcf0aa89bcd7 | 26 | self->trig1=1; // trigger high |
edy05 | 7:dcf0aa89bcd7 | 27 | //printf("trig\n\r"); |
edy05 | 1:53657de3246f | 28 | wait_us(10); |
edy05 | 7:dcf0aa89bcd7 | 29 | //printf("wait\n\r"); |
edy05 | 7:dcf0aa89bcd7 | 30 | self->trig1=0; // trigger low |
edy05 | 3:81512ca9a13c | 31 | // start pulseIN |
edy05 | 7:dcf0aa89bcd7 | 32 | self->timer1.reset(); |
edy05 | 7:dcf0aa89bcd7 | 33 | self->timer1.start(); |
edy05 | 7:dcf0aa89bcd7 | 34 | while(!self->echo1){ |
edy05 | 7:dcf0aa89bcd7 | 35 | if(self->timer1.read_us() > self->limit){ |
edy05 | 3:81512ca9a13c | 36 | //printf("moc1\n\r"); |
edy05 | 6:7f98f258e8ef | 37 | // if no response from distance sensor -> let the quadcopter fall |
edy05 | 7:dcf0aa89bcd7 | 38 | break; |
edy05 | 3:81512ca9a13c | 39 | } |
edy05 | 3:81512ca9a13c | 40 | } |
edy05 | 7:dcf0aa89bcd7 | 41 | if(self->timer1.read_us() < self->limit){ |
edy05 | 7:dcf0aa89bcd7 | 42 | self->timer1.stop(); |
edy05 | 7:dcf0aa89bcd7 | 43 | self->timer1.reset(); //reset timer |
edy05 | 7:dcf0aa89bcd7 | 44 | self->timer1.start(); |
edy05 | 7:dcf0aa89bcd7 | 45 | while(self->echo1){ |
edy05 | 7:dcf0aa89bcd7 | 46 | if(self->timer1.read_us() > self->limit){ |
edy05 | 3:81512ca9a13c | 47 | //printf("moc2\n\r"); |
edy05 | 6:7f98f258e8ef | 48 | // if no response from distance sensor -> let the quadcopter fall |
edy05 | 7:dcf0aa89bcd7 | 49 | break; |
edy05 | 3:81512ca9a13c | 50 | } |
edy05 | 3:81512ca9a13c | 51 | } |
edy05 | 7:dcf0aa89bcd7 | 52 | self->timer1.stop(); |
edy05 | 7:dcf0aa89bcd7 | 53 | self->distan1 = self->timer1.read_us() / 58; |
edy05 | 7:dcf0aa89bcd7 | 54 | } |
edy05 | 7:dcf0aa89bcd7 | 55 | //printf("end\n\r"); |
edy05 | 7:dcf0aa89bcd7 | 56 | } |
edy05 | 7:dcf0aa89bcd7 | 57 | |
edy05 | 7:dcf0aa89bcd7 | 58 | void HCSR04::threadWorker2(void const *p) |
edy05 | 7:dcf0aa89bcd7 | 59 | { |
edy05 | 7:dcf0aa89bcd7 | 60 | HCSR04* self = (HCSR04*)p; |
edy05 | 7:dcf0aa89bcd7 | 61 | |
edy05 | 7:dcf0aa89bcd7 | 62 | self->trig2=1; // trigger high |
edy05 | 7:dcf0aa89bcd7 | 63 | wait_us(10); |
edy05 | 7:dcf0aa89bcd7 | 64 | self->trig2=0; // trigger low |
edy05 | 7:dcf0aa89bcd7 | 65 | // start pulseIN |
edy05 | 7:dcf0aa89bcd7 | 66 | self->timer2.reset(); |
edy05 | 7:dcf0aa89bcd7 | 67 | self->timer2.start(); |
edy05 | 7:dcf0aa89bcd7 | 68 | while(!self->echo2){ |
edy05 | 7:dcf0aa89bcd7 | 69 | if(self->timer2.read_us() > self->limit){ |
edy05 | 7:dcf0aa89bcd7 | 70 | //printf("moc1\n\r"); |
edy05 | 7:dcf0aa89bcd7 | 71 | // if no response from distance sensor -> let the quadcopter fall |
edy05 | 7:dcf0aa89bcd7 | 72 | //self->distan2 = 500; |
edy05 | 7:dcf0aa89bcd7 | 73 | break; |
edy05 | 7:dcf0aa89bcd7 | 74 | } |
edy05 | 7:dcf0aa89bcd7 | 75 | } |
edy05 | 7:dcf0aa89bcd7 | 76 | if(self->timer2.read_us() < self->limit){ |
edy05 | 7:dcf0aa89bcd7 | 77 | self->timer2.stop(); |
edy05 | 7:dcf0aa89bcd7 | 78 | self->timer2.reset(); //reset timer |
edy05 | 7:dcf0aa89bcd7 | 79 | self->timer2.start(); |
edy05 | 7:dcf0aa89bcd7 | 80 | while(self->echo2){ |
edy05 | 7:dcf0aa89bcd7 | 81 | if(self->timer2.read_us() > self->limit){ |
edy05 | 7:dcf0aa89bcd7 | 82 | //printf("moc2\n\r"); |
edy05 | 7:dcf0aa89bcd7 | 83 | // if no response from distance sensor -> let the quadcopter fall |
edy05 | 7:dcf0aa89bcd7 | 84 | //self->distan2 = 500; |
edy05 | 7:dcf0aa89bcd7 | 85 | break; |
edy05 | 7:dcf0aa89bcd7 | 86 | } |
edy05 | 7:dcf0aa89bcd7 | 87 | } |
edy05 | 7:dcf0aa89bcd7 | 88 | self->timer2.stop(); |
edy05 | 7:dcf0aa89bcd7 | 89 | self->distan2 = self->timer2.read_us() / 58; |
edy05 | 3:81512ca9a13c | 90 | } |
edy05 | 1:53657de3246f | 91 | } |
edy05 | 1:53657de3246f | 92 | |
edy05 | 7:dcf0aa89bcd7 | 93 | uint16_t HCSR04::getDistan1(){ |
edy05 | 7:dcf0aa89bcd7 | 94 | return distan1; |
edy05 | 7:dcf0aa89bcd7 | 95 | } |
edy05 | 7:dcf0aa89bcd7 | 96 | |
edy05 | 7:dcf0aa89bcd7 | 97 | uint16_t HCSR04::getDistan2(){ |
edy05 | 7:dcf0aa89bcd7 | 98 | return distan2; |
antoniolinux | 0:86b2086be101 | 99 | } |
antoniolinux | 0:86b2086be101 | 100 |