hcsr04.cpp

00001 #include "hcsr04.h"
00002 #include "definitions.h"
00003 #include "mbed.h"
00004 #include "rtos.h"
00005 /*
00006 *HCSR04.cpp
00007 */
00008 HCSR04::HCSR04(PinName t1, PinName e1) : trig1(t1), echo1(e1){
00009     
00010     int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; //ms
00011     
00012     limit = (rate * 1000 - rate * 200);
00013     
00014     threadUpdateTimer1 = new RtosTimer(threadWorker1, osTimerPeriodic, this);
00015     threadUpdateTimer1->start(rate);
00016     
00017     id = Thread::gettid();
00018     printf("down sonic gettid 0x%08X \n\r", id);
00019     
00020     //threadUpdateTimer2 = new RtosTimer(threadWorker2, osTimerPeriodic, this);
00021     //threadUpdateTimer2->start(rate);
00022 }
00023 
00024 void HCSR04::threadWorker1(void const *p)
00025 {
00026     HCSR04* self = (HCSR04*)p;
00027     
00028         //printf("start\n\r");
00029         self->trig1=1;   //  trigger high
00030         //printf("trig\n\r");
00031         wait_us(10);
00032         //printf("wait\n\r");
00033         self->trig1=0;  // trigger low
00034         // start pulseIN
00035         self->timerLimit.reset();
00036         self->timerLimit.start();
00037         while(!self->echo1){
00038             if(self->timerLimit.read_us() > self->limit){
00039                 //printf("moc1\n\r");
00040                 break;
00041             }    
00042         }
00043         if(self->timerLimit.read_us() < self->limit){
00044             //self->timer1.stop();
00045             self->timer1.reset();  //reset timer
00046             self->timer1.start();
00047             while(self->echo1){
00048                 if(self->timerLimit.read_us() > self->limit){
00049                     //printf("moc2\n\r");
00050                     break;
00051                 }
00052             }
00053             self->timer1.stop();
00054             self->distan1 = self->timer1.read_us() / 58;
00055         }
00056         //printf("end\n\r");
00057 }
00058 
00059 
00060 float HCSR04::getDistan1(){
00061     return distan1;    
00062 }
00063 
00064