File content as of revision 13:e9b99635aa2d:

#include "hcsr04.h"
#include "definitions.h"
#include "mbed.h"
#include "rtos.h"
/*
*HCSR04.cpp
*/
HCSR04::HCSR04(PinName t1, PinName e1) : trig1(t1), echo1(e1){
    
    int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; //ms
    
    limit = (rate * 1000 - rate * 200);
    
    threadUpdateTimer1 = new RtosTimer(threadWorker1, osTimerPeriodic, this);
    threadUpdateTimer1->start(rate);
    
    id = Thread::gettid();
    printf("down sonic gettid 0x%08X \n\r", id);
    
    //threadUpdateTimer2 = new RtosTimer(threadWorker2, osTimerPeriodic, this);
    //threadUpdateTimer2->start(rate);
}

void HCSR04::threadWorker1(void const *p)
{
    HCSR04* self = (HCSR04*)p;
    
        //printf("start\n\r");
        self->trig1=1;   //  trigger high
        //printf("trig\n\r");
        wait_us(10);
        //printf("wait\n\r");
        self->trig1=0;  // trigger low
        // start pulseIN
        self->timerLimit.reset();
        self->timerLimit.start();
        while(!self->echo1){
            if(self->timerLimit.read_us() > self->limit){
                //printf("moc1\n\r");
                break;
            }    
        }
        if(self->timerLimit.read_us() < self->limit){
            //self->timer1.stop();
            self->timer1.reset();  //reset timer
            self->timer1.start();
            while(self->echo1){
                if(self->timerLimit.read_us() > self->limit){
                    //printf("moc2\n\r");
                    break;
                }
            }
            self->timer1.stop();
            self->distan1 = self->timer1.read_us() / 58;
        }
        //printf("end\n\r");
}


float HCSR04::getDistan1(){
    return distan1;    
}