Eduard Medla / HCSR04

1 sensor using RTOS Timer, another 2 over RTOS Semaphore

Dependents:   Autonomous_quadcopter

Fork of HCSR04 by Antoniolinux B.

hcsr04.cpp@11:49a0feca5d71, 2018-03-04 (annotated)

Committer:
edy05
Date:
Sun Mar 04 15:17:12 2018 +0000
Revision:
11:49a0feca5d71
Parent:
8:0a10bacaf501
Child:
12:6f6469b2f016
distance is float

Who changed what in which revision?

UserRevisionLine numberNew 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 8:0a10bacaf501 10 int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; //ms
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 8:0a10bacaf501 32 self->timerLimit.reset();
edy05 8:0a10bacaf501 33 self->timerLimit.start();
edy05 7:dcf0aa89bcd7 34 while(!self->echo1){
edy05 8:0a10bacaf501 35 if(self->timerLimit.read_us() > self->limit){
edy05 3:81512ca9a13c 36 //printf("moc1\n\r");
edy05 7:dcf0aa89bcd7 37 break;
edy05 3:81512ca9a13c 38 }
edy05 3:81512ca9a13c 39 }
edy05 8:0a10bacaf501 40 if(self->timerLimit.read_us() < self->limit){
edy05 8:0a10bacaf501 41 //self->timer1.stop();
edy05 7:dcf0aa89bcd7 42 self->timer1.reset(); //reset timer
edy05 7:dcf0aa89bcd7 43 self->timer1.start();
edy05 7:dcf0aa89bcd7 44 while(self->echo1){
edy05 8:0a10bacaf501 45 if(self->timerLimit.read_us() > self->limit){
edy05 3:81512ca9a13c 46 //printf("moc2\n\r");
edy05 7:dcf0aa89bcd7 47 break;
edy05 3:81512ca9a13c 48 }
edy05 3:81512ca9a13c 49 }
edy05 7:dcf0aa89bcd7 50 self->timer1.stop();
edy05 7:dcf0aa89bcd7 51 self->distan1 = self->timer1.read_us() / 58;
edy05 7:dcf0aa89bcd7 52 }
edy05 7:dcf0aa89bcd7 53 //printf("end\n\r");
edy05 7:dcf0aa89bcd7 54 }
edy05 7:dcf0aa89bcd7 55
edy05 7:dcf0aa89bcd7 56 void HCSR04::threadWorker2(void const *p)
edy05 7:dcf0aa89bcd7 57 {
edy05 7:dcf0aa89bcd7 58 HCSR04* self = (HCSR04*)p;
edy05 7:dcf0aa89bcd7 59
edy05 7:dcf0aa89bcd7 60 self->trig2=1; // trigger high
edy05 7:dcf0aa89bcd7 61 wait_us(10);
edy05 7:dcf0aa89bcd7 62 self->trig2=0; // trigger low
edy05 7:dcf0aa89bcd7 63 // start pulseIN
edy05 7:dcf0aa89bcd7 64 self->timer2.reset();
edy05 7:dcf0aa89bcd7 65 self->timer2.start();
edy05 7:dcf0aa89bcd7 66 while(!self->echo2){
edy05 7:dcf0aa89bcd7 67 if(self->timer2.read_us() > self->limit){
edy05 7:dcf0aa89bcd7 68 //printf("moc1\n\r");
edy05 7:dcf0aa89bcd7 69 // if no response from distance sensor -> let the quadcopter fall
edy05 7:dcf0aa89bcd7 70 //self->distan2 = 500;
edy05 7:dcf0aa89bcd7 71 break;
edy05 7:dcf0aa89bcd7 72 }
edy05 7:dcf0aa89bcd7 73 }
edy05 7:dcf0aa89bcd7 74 if(self->timer2.read_us() < self->limit){
edy05 7:dcf0aa89bcd7 75 self->timer2.stop();
edy05 7:dcf0aa89bcd7 76 self->timer2.reset(); //reset timer
edy05 7:dcf0aa89bcd7 77 self->timer2.start();
edy05 7:dcf0aa89bcd7 78 while(self->echo2){
edy05 7:dcf0aa89bcd7 79 if(self->timer2.read_us() > self->limit){
edy05 7:dcf0aa89bcd7 80 //printf("moc2\n\r");
edy05 7:dcf0aa89bcd7 81 // if no response from distance sensor -> let the quadcopter fall
edy05 7:dcf0aa89bcd7 82 //self->distan2 = 500;
edy05 7:dcf0aa89bcd7 83 break;
edy05 7:dcf0aa89bcd7 84 }
edy05 7:dcf0aa89bcd7 85 }
edy05 7:dcf0aa89bcd7 86 self->timer2.stop();
edy05 7:dcf0aa89bcd7 87 self->distan2 = self->timer2.read_us() / 58;
edy05 3:81512ca9a13c 88 }
edy05 1:53657de3246f 89 }
edy05 1:53657de3246f 90
edy05 11:49a0feca5d71 91 float HCSR04::getDistan1(){
edy05 7:dcf0aa89bcd7 92 return distan1;
edy05 7:dcf0aa89bcd7 93 }
edy05 7:dcf0aa89bcd7 94
edy05 7:dcf0aa89bcd7 95 uint16_t HCSR04::getDistan2(){
edy05 7:dcf0aa89bcd7 96 return distan2;
antoniolinux 0:86b2086be101 97 }
antoniolinux 0:86b2086be101 98