From 4e7fa5aade14c4864d254400695e44f8e0f05074 Mon Sep 17 00:00:00 2001
From: Johan Jansen <jnsn.johan@gmail.com>
Date: Tue, 24 Mar 2015 12:23:15 +0100
Subject: [PATCH] LidarLite: Add uORB handling in PWM drivers

---
 src/drivers/ll40ls/LidarLitePWM.cpp | 33 +++++++++++++++++++++++++----
 src/drivers/ll40ls/LidarLitePWM.h   |  2 ++
 2 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp
index 7396ffd121..5486bbe62c 100644
--- a/src/drivers/ll40ls/LidarLitePWM.cpp
+++ b/src/drivers/ll40ls/LidarLitePWM.cpp
@@ -40,6 +40,7 @@
  */
 #include "LidarLitePWM.h"
 #include <stdio.h>
+#include <drivers/drv_hrt.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef __cplusplus
@@ -48,7 +49,9 @@ static const int ERROR = -1;
 
 LidarLitePWM::LidarLitePWM() :
     _pwmSub(-1),
-    _pwm{}
+    _pwm{},
+    _rangePub(-1),
+    _range{}
 {
 
 }
@@ -61,6 +64,10 @@ int LidarLitePWM::init()
         return ERROR;
     }
 
+    _range.type = RANGE_FINDER_TYPE_LASER;
+    _range.valid = false;
+    _rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range);
+
     return OK;
 }
 
@@ -76,15 +83,33 @@ void LidarLitePWM::print_registers()
 
 void LidarLitePWM::start()
 {
-
+    //TODO: start measurement task
 }
 
 void LidarLitePWM::stop()
 {
-
+    //TODO: stop measurement task
 }
 
 int LidarLitePWM::measure()
 {
+    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;
+
+        orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
+    }
+
     return OK;
-}
\ No newline at end of file
+}
diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h
index 75d8be245a..ca43662efa 100644
--- a/src/drivers/ll40ls/LidarLitePWM.h
+++ b/src/drivers/ll40ls/LidarLitePWM.h
@@ -73,4 +73,6 @@ protected:
 private:
     int _pwmSub;
     pwm_input_s _pwm;
+    orb_advert_t _rangePub;
+    range_finder_report _range;
 };