Eduard Medla / HCSR04

1 sensor using RTOS Timer, another 2 over RTOS Semaphore

Dependents:   Autonomous_quadcopter

Fork of HCSR04 by Antoniolinux B.

hcsr04.cpp@4:34023f76fc4f, 2018-01-21 (annotated)

Committer:
edy05
Date:
Sun Jan 21 17:56:47 2018 +0000
Revision:
4:34023f76fc4f
Parent:
3:81512ca9a13c 
rate calculated by frequency

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antoniolinux 0:86b2086be101 1 #include "hcsr04.h"
edy05 4:34023f76fc4f 2 #include "definitions.h"
antoniolinux 0:86b2086be101 3 #include "mbed.h"
edy05 1:53657de3246f 4 #include "rtos.h"
edy05 4:34023f76fc4f 5
edy05 4:34023f76fc4f 6
antoniolinux 0:86b2086be101 7 /*
antoniolinux 0:86b2086be101 8 *HCSR04.cpp
antoniolinux 0:86b2086be101 9 */
edy05 1:53657de3246f 10 HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) {
edy05 3:81512ca9a13c 11 //thread = new Thread(threadWorker, this);
edy05 3:81512ca9a13c 12 threadUpdateTimer = new RtosTimer(threadWorker, osTimerPeriodic, this);
edy05 4:34023f76fc4f 13 int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
edy05 4:34023f76fc4f 14 threadUpdateTimer->start(rate);
edy05 1:53657de3246f 15 }
edy05 1:53657de3246f 16
edy05 1:53657de3246f 17 void HCSR04::threadWorker(void const *p)
edy05 1:53657de3246f 18 {
edy05 1:53657de3246f 19 HCSR04* self = (HCSR04*)p;
edy05 1:53657de3246f 20
edy05 1:53657de3246f 21 self->trig=1; // trigger high
edy05 1:53657de3246f 22 wait_us(10);
edy05 1:53657de3246f 23 self->trig=0; // trigger low
edy05 3:81512ca9a13c 24 // start pulseIN
edy05 3:81512ca9a13c 25 self->timer.reset();
edy05 1:53657de3246f 26 self->timer.start();
edy05 3:81512ca9a13c 27 while(!self->echo){
edy05 3:81512ca9a13c 28 if(self->timer.read_us() > 4500){
edy05 3:81512ca9a13c 29 //printf("moc1\n\r");
edy05 3:81512ca9a13c 30 break;
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 3:81512ca9a13c 40 break;
edy05 3:81512ca9a13c 41 }
edy05 3:81512ca9a13c 42 }
edy05 3:81512ca9a13c 43 self->timer.stop();
edy05 3:81512ca9a13c 44 self->distan = self->timer.read_us() / 58;
edy05 3:81512ca9a13c 45 }
edy05 1:53657de3246f 46 }
edy05 1:53657de3246f 47
edy05 1:53657de3246f 48 uint16_t HCSR04::getDistan(){
edy05 1:53657de3246f 49 return distan;
antoniolinux 0:86b2086be101 50 }
antoniolinux 0:86b2086be101 51
antoniolinux 0:86b2086be101 52 //return distance in cm
edy05 1:53657de3246f 53 uint16_t HCSR04::getDistance(){
edy05 1:53657de3246f 54 distance_cm = distan/58;
edy05 1:53657de3246f 55 return distance_cm;
antoniolinux 0:86b2086be101 56
antoniolinux 0:86b2086be101 57 }