Eduard Medla / HCSR04

1 sensor using RTOS Timer, another 2 over RTOS Semaphore

Dependents:   Autonomous_quadcopter

Fork of HCSR04 by Antoniolinux B.

hcsr04.cpp@5:02948ae78be7, 2018-02-04 (annotated)

Committer:
edy05
Date:
Sun Feb 04 15:20:37 2018 +0000
Revision:
5:02948ae78be7
Parent:
3:81512ca9a13c
Child:
6:7f98f258e8ef
better freqeuency control;

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 3:81512ca9a13c 28 break;
edy05 3:81512ca9a13c 29 }
edy05 3:81512ca9a13c 30 }
edy05 3:81512ca9a13c 31 if(self->timer.read_us() < 4500){
edy05 3:81512ca9a13c 32 self->timer.stop();
edy05 3:81512ca9a13c 33 self->timer.reset(); //reset timer
edy05 3:81512ca9a13c 34 self->timer.start();
edy05 3:81512ca9a13c 35 while(self->echo){
edy05 3:81512ca9a13c 36 if(self->timer.read_us() > 4500){
edy05 3:81512ca9a13c 37 //printf("moc2\n\r");
edy05 3:81512ca9a13c 38 break;
edy05 3:81512ca9a13c 39 }
edy05 3:81512ca9a13c 40 }
edy05 3:81512ca9a13c 41 self->timer.stop();
edy05 3:81512ca9a13c 42 self->distan = self->timer.read_us() / 58;
edy05 3:81512ca9a13c 43 }
edy05 1:53657de3246f 44 }
edy05 1:53657de3246f 45
edy05 1:53657de3246f 46 uint16_t HCSR04::getDistan(){
edy05 1:53657de3246f 47 return distan;
antoniolinux 0:86b2086be101 48 }
antoniolinux 0:86b2086be101 49
antoniolinux 0:86b2086be101 50 //return distance in cm
edy05 1:53657de3246f 51 uint16_t HCSR04::getDistance(){
edy05 1:53657de3246f 52 distance_cm = distan/58;
edy05 1:53657de3246f 53 return distance_cm;
antoniolinux 0:86b2086be101 54
antoniolinux 0:86b2086be101 55 }