File content as of revision 6:7f98f258e8ef:

#include "hcsr04.h"
#include "definitions.h"
#include "mbed.h"
#include "rtos.h"
/*
*HCSR04.cpp
*/
HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) {
    //thread = new Thread(threadWorker, this);
    threadUpdateTimer = new RtosTimer(threadWorker, osTimerPeriodic, this);
    int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
    threadUpdateTimer->start(rate);
}

void HCSR04::threadWorker(void const *p)
{
    HCSR04* self = (HCSR04*)p;
    
        self->trig=1;   //  trigger high
        wait_us(10);
        self->trig=0;  // trigger low
        // start pulseIN
        self->timer.reset();
        self->timer.start();
        while(!self->echo){
            if(self->timer.read_us() > 4500){
                //printf("moc1\n\r");
                // if no response from distance sensor -> let the quadcopter fall
                self->distan = 500;
                return;
            }    
        }
        if(self->timer.read_us() < 4500){
            self->timer.stop();
            self->timer.reset();  //reset timer
            self->timer.start();
            while(self->echo){
                if(self->timer.read_us() > 4500){
                    //printf("moc2\n\r");
                    // if no response from distance sensor -> let the quadcopter fall
                    self->distan = 500;
                    return;
                }
            }
            self->timer.stop();
            self->distan = self->timer.read_us() / 58;
        }
}

uint16_t HCSR04::getDistan(){
    return distan;    
}
 
//return distance in cm 
uint16_t HCSR04::getDistance(){
    distance_cm = distan/58;
    return distance_cm;

}