File content as of revision 8:0a10bacaf501:

#include "hcsr04.h"
#include "definitions.h"
#include "mbed.h"
#include "rtos.h"
/*
*HCSR04.cpp
*/
HCSR04::HCSR04(PinName t1, PinName e1, PinName t2, PinName e2) : trig1(t1), echo1(e1), trig2(t2), echo2(e2) {
    
    int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; //ms
    
    limit = (rate * 1000 - rate * 200);
    
    threadUpdateTimer1 = new RtosTimer(threadWorker1, osTimerPeriodic, this);
    threadUpdateTimer1->start(rate);
    
    //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");
}

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

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

uint16_t HCSR04::getDistan2(){
    return distan2;    
}