Diff: LidarLitePwm.cpp

Revision:

3:0b1f4404cb21

Parent:

2:be66a4cd86c0

--- a/LidarLitePwm.cpp	Wed Mar 18 09:17:13 2015 +0000
+++ b/LidarLitePwm.cpp	Wed Apr 01 11:18:45 2015 +0000
@@ -4,6 +4,7 @@
 {
     _pulseStartTime = 0;
     _range = 0;
+    _lidarFilter = new filter(5);
     _timer.start();
     _interrupt.rise(this, &LidarLitePwm::pulseStart);
     _interrupt.fall(this, &LidarLitePwm::pulseStop);
@@ -13,7 +14,8 @@
 
 int LidarLitePwm::read()
 {
-    return _range;
+    if(_range < 30) return 0;
+    else return _range - 30;
 }
 
 LidarLitePwm::operator int()
@@ -30,5 +32,6 @@
 {
     int endTime = _timer.read_us();
     if (endTime < _pulseStartTime) return; // Escape if there's been a roll over
-    _range = (endTime - _pulseStartTime) / 10; // 10uS per CM
+    int range = (endTime - _pulseStartTime) / 10; // 10uS per CM
+    _range = _lidarFilter->process(range);
 }
\ No newline at end of file