Eduard Medla / HCSR04

1 sensor using RTOS Timer, another 2 over RTOS Semaphore

Dependents:   Autonomous_quadcopter

Fork of HCSR04 by Antoniolinux B.

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?

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 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