diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h
index a00f81740d..1ea595909d 100644
--- a/src/drivers/ll40ls/LidarLite.h
+++ b/src/drivers/ll40ls/LidarLite.h
@@ -94,6 +94,7 @@ protected:
     uint32_t            getMeasureTicks() const;
 
     virtual int         measure() = 0;
+    virtual int         collect() = 0;
 
     virtual int         reset_sensor() = 0;
 
diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp
index 5486bbe62c..50ee4880b9 100644
--- a/src/drivers/ll40ls/LidarLitePWM.cpp
+++ b/src/drivers/ll40ls/LidarLitePWM.cpp
@@ -48,6 +48,7 @@ static const int ERROR = -1;
 #endif
 
 LidarLitePWM::LidarLitePWM() :
+    _terminateRequested(false),
     _pwmSub(-1),
     _pwm{},
     _rangePub(-1),
@@ -74,6 +75,8 @@ int LidarLitePWM::init()
 void LidarLitePWM::print_info()
 {
     printf("poll interval:  %u ticks\n", getMeasureTicks());
+    printf("distance: %ucm (0x%04x)\n",
+           (unsigned)_range.distance, (unsigned)_range.distance);
 }
 
 void LidarLitePWM::print_registers()
@@ -83,33 +86,57 @@ void LidarLitePWM::print_registers()
 
 void LidarLitePWM::start()
 {
-    //TODO: start measurement task
+
 }
 
 void LidarLitePWM::stop()
 {
     //TODO: stop measurement task
+    _terminateRequested = true;
 }
 
 int LidarLitePWM::measure()
 {
+    int result = OK;
+
+    _range.error_count = _pwm.error_count;
+    _range.maximum_distance = get_maximum_distance();
+    _range.minimum_distance = get_minimum_distance();
+    _range.distance = _pwm.pulse_width / 1000.0f;   //10 usec = 1 cm distance for LIDAR-Lite
+    _range.distance_vector[0] = _range.distance;
+    _range.just_updated = 0;
+    _range.valid = true;
+
+    //TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0)
+    if(_range.distance <= 0.0f) {
+        _range.valid = false;
+        _range.error_count++;
+        result = ERROR;
+    }
+
+    orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
+
+    return result;
+}
+
+int LidarLitePWM::collect()
+{
+    //Check PWM
     bool update;
     orb_check(_pwmSub, &update);
-
     if(update) {
         orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm);
-
         _range.timestamp = hrt_absolute_time();
-        _range.error_count = _pwm.error_count;
-        _range.valid = true;
-        _range.maximum_distance = get_maximum_distance();
-        _range.minimum_distance = get_minimum_distance();
-        _range.distance = _pwm.pulse_width / 1000.0f;   //10 usec = 1 cm distance for LIDAR-Lite
-        _range.distance_vector[0] = _range.distance;
-        _range.just_updated = 0;
+        return OK;
+    }
 
+    //Timeout readings after 0.2 seconds and mark as invalid
+    if(hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT*2) {
+        _range.timestamp = hrt_absolute_time();
+        _range.valid = false;
         orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
+        return ERROR;
     }
 
-    return OK;
+    return EAGAIN;
 }
diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h
index ca43662efa..0b85d53abc 100644
--- a/src/drivers/ll40ls/LidarLitePWM.h
+++ b/src/drivers/ll40ls/LidarLitePWM.h
@@ -70,7 +70,12 @@ public:
 protected:
     int measure() override;
 
+    int collect() override;
+
+    void task_main_trampoline(int argc, char *argv[]);
+
 private:
+    bool _terminateRequested;
     int _pwmSub;
     pwm_input_s _pwm;
     orb_advert_t _rangePub;