HCSR04.cpp

00001 #include "mbed.h"
00002 #include "HCSR04.h"
00003 extern Serial pc;
00004 
00005 HCSR04::HCSR04(PinName echoPin, PinName triggerPin, int windowsize, int idx) : 
00006                 echo(echoPin), trigger(triggerPin), _windowsize(windowsize), index(idx) {
00007     init();
00008 }
00009 
00010 void HCSR04::init() {
00011     distance = 0;      // initial distance
00012     minDistance = 3;
00013     maxDistance = 400;
00014     timerStarted = false;
00015     newDataReady = 0;
00016     distance_v.reserve(_windowsize);
00017     distance_v.assign(_windowsize, 0);
00018     distance_filter = 0;
00019     _distance_filterpre = 0;
00020     _ii = 1;
00021     timeout_echo = false;
00022     timeout_manager.attach(callback(this,&HCSR04::SetTimeoutRoutine),5);
00023 }
00024 
00025 void HCSR04::startTimer() {
00026     echo.rise(NULL);
00027     timer.start(); // start the timer
00028     echoFallTimeout.attach_us(callback(this, &HCSR04::stopTimer), 58.8235 * maxDistance); // in case echo fall does not occur
00029     echo.fall(callback(this, &HCSR04::stopTimer));
00030 }
00031 
00032 void HCSR04::stopTimer()
00033 {
00034     timer.stop();
00035     distance = timer.read() * 1e6 / 58.8235;
00036     timer.reset();
00037     echoRiseTimeout.detach();
00038     echoFallTimeout.detach();
00039     echo.fall(NULL);
00040     newDataReady = true;
00041     timeout_echo = false;
00042 }
00043 
00044 void HCSR04::startMeasurement() {
00045     newDataReady = false;
00046     echo.rise(callback(this, &HCSR04::startTimer));
00047 }
00048 
00049 void HCSR04::NoEchoTimeout()
00050 {
00051     timeout_echo = true;
00052     newDataReady = true;
00053 }
00054 
00055 void HCSR04::SetTimeoutRoutine()
00056 {
00057     echoRiseTimeout.attach(callback(this, &HCSR04::NoEchoTimeout),HCSR04_TIMEOUT);
00058 }
00059 
00060 float HCSR04::getDistance_cm() {
00061     
00062     if(timeout_echo)
00063     {
00064         return -1.0f;
00065     }
00066     
00067     if(distance < minDistance)
00068     {
00069         distance = minDistance;
00070     }
00071     MAF();
00072     return distance_filter;
00073 }
00074 
00075 float HCSR04::getDistance_raw() {
00076     return distance;
00077 }
00078 
00079 float HCSR04::getDistance_mm() {
00080     return HCSR04::getDistance_cm() * 10;
00081 }
00082 
00083 bool HCSR04::isNewDataReady() {
00084     return newDataReady;
00085 }
00086 
00087 void HCSR04::setRanges(float minRange, float maxRange) {
00088     if (minRange < maxRange)
00089     {
00090         if (minRange >= 2 && minRange < 400) // bug from revs. 4 and 5 corrected
00091             minDistance = minRange;
00092         if (maxRange <= 400)
00093             maxDistance = maxRange;
00094     }
00095 }
00096 
00097 float HCSR04::getMinRange()
00098 {
00099     return minDistance;
00100 }
00101 
00102 float HCSR04::getMaxRange()
00103 {
00104     return maxDistance;
00105 }
00106 
00107 void HCSR04::MAF() {
00108     float sum = 0;
00109     float beta = 0.9;
00110     if(_ii <= _windowsize) { // pass when data is not ready
00111         if(_ii <= 1) distance_v[_ii-1] = distance;
00112         _ii++;
00113         return;
00114     }
00115     
00116     if(_ii == _windowsize + 1) {
00117         for(int j = 0 ; j < _windowsize; j++) {
00118             sum += distance_v[j];
00119         }
00120         _distance_filterpre = sum / _windowsize;
00121     }
00122     distance_filter = _distance_filterpre + (distance - distance_v[(_ii - 1)%_windowsize]) / _windowsize;  // x_bar[k] = x_bar[k-1] + (x[k] + x[k-n])/n
00123     distance_v[(_ii - 1)%_windowsize] = distance;  // x[k-n] = x[k]
00124     _distance_filterpre = distance_filter;  // x_bar[k-1] = x[k]
00125     distance_filter = distance_filter * (1-beta) + _distance_filterpre*beta;
00126     _ii++;
00127     
00128 }
00129 
00130 void HCSR04::SetTrigger(int level)
00131 {
00132     trigger = level;
00133 }
00134 
00135 void HCSR04::turnOnTrigger()
00136 {
00137     SetTrigger(1);
00138 }
00139 
00140 void HCSR04::turnOffTrigger() {
00141     SetTrigger(0);
00142 }
00143 
00144 // EOF