Eduard Medla / HCSR04

1 sensor using RTOS Timer, another 2 over RTOS Semaphore

Dependents:   Autonomous_quadcopter

Fork of HCSR04 by Antoniolinux B.

hcsr04.cpp@6:7f98f258e8ef, 2018-02-18 (annotated)

Committer:
edy05
Date:
Sun Feb 18 17:10:25 2018 +0000
Revision:
6:7f98f258e8ef
Parent:
5:02948ae78be7
Child:
7:dcf0aa89bcd7
distance == 500, if no response from ultrasonic sensor (or too long)

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 1:53657de3246f 8 HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) {
edy05 3:81512ca9a13c 9 //thread = new Thread(threadWorker, this);
edy05 3:81512ca9a13c 10 threadUpdateTimer = new RtosTimer(threadWorker, osTimerPeriodic, this);
edy05 5:02948ae78be7 11 int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
edy05 5:02948ae78be7 12 threadUpdateTimer->start(rate);
edy05 1:53657de3246f 13 }
edy05 1:53657de3246f 14
edy05 1:53657de3246f 15 void HCSR04::threadWorker(void const *p)
edy05 1:53657de3246f 16 {
edy05 1:53657de3246f 17 HCSR04* self = (HCSR04*)p;
edy05 1:53657de3246f 18
edy05 1:53657de3246f 19 self->trig=1; // trigger high
edy05 1:53657de3246f 20 wait_us(10);
edy05 1:53657de3246f 21 self->trig=0; // trigger low
edy05 3:81512ca9a13c 22 // start pulseIN
edy05 3:81512ca9a13c 23 self->timer.reset();
edy05 1:53657de3246f 24 self->timer.start();
edy05 3:81512ca9a13c 25 while(!self->echo){
edy05 3:81512ca9a13c 26 if(self->timer.read_us() > 4500){
edy05 3:81512ca9a13c 27 //printf("moc1\n\r");
edy05 6:7f98f258e8ef 28 // if no response from distance sensor -> let the quadcopter fall
edy05 6:7f98f258e8ef 29 self->distan = 500;
edy05 6:7f98f258e8ef 30 return;
edy05 3:81512ca9a13c 31 }
edy05 3:81512ca9a13c 32 }
edy05 3:81512ca9a13c 33 if(self->timer.read_us() < 4500){
edy05 3:81512ca9a13c 34 self->timer.stop();
edy05 3:81512ca9a13c 35 self->timer.reset(); //reset timer
edy05 3:81512ca9a13c 36 self->timer.start();
edy05 3:81512ca9a13c 37 while(self->echo){
edy05 3:81512ca9a13c 38 if(self->timer.read_us() > 4500){
edy05 3:81512ca9a13c 39 //printf("moc2\n\r");
edy05 6:7f98f258e8ef 40 // if no response from distance sensor -> let the quadcopter fall
edy05 6:7f98f258e8ef 41 self->distan = 500;
edy05 6:7f98f258e8ef 42 return;
edy05 3:81512ca9a13c 43 }
edy05 3:81512ca9a13c 44 }
edy05 3:81512ca9a13c 45 self->timer.stop();
edy05 3:81512ca9a13c 46 self->distan = self->timer.read_us() / 58;
edy05 3:81512ca9a13c 47 }
edy05 1:53657de3246f 48 }
edy05 1:53657de3246f 49
edy05 1:53657de3246f 50 uint16_t HCSR04::getDistan(){
edy05 1:53657de3246f 51 return distan;
antoniolinux 0:86b2086be101 52 }
antoniolinux 0:86b2086be101 53
antoniolinux 0:86b2086be101 54 //return distance in cm
edy05 1:53657de3246f 55 uint16_t HCSR04::getDistance(){
edy05 1:53657de3246f 56 distance_cm = distan/58;
edy05 1:53657de3246f 57 return distance_cm;
antoniolinux 0:86b2086be101 58
antoniolinux 0:86b2086be101 59 }