ll40ls cleanup and create PX4Rangerfinder helper class

src/drivers/distance_sensor/ll40ls/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -35,9 +35,13 @@ px4_add_module(
 	MAIN ll40ls
 	COMPILE_FLAGS
 		-Wno-cast-align # TODO: fix and enable
+	STACK_MAIN
+		1500
 	SRCS
 		ll40ls.cpp
 		LidarLite.cpp
 		LidarLiteI2C.cpp
 		LidarLitePWM.cpp
+	DEPENDS
+		drivers_rangefinder
 	)

src/drivers/distance_sensor/ll40ls/LidarLite.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,64 +41,35 @@
 
 #include "LidarLite.h"
 
-int LidarLite::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+LidarLite::LidarLite(uint8_t rotation) :
+	_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls: read")),
+	_sample_interval_perf(perf_alloc(PC_ELAPSED, "ll40ls: interval")),
+	_comms_errors(perf_alloc(PC_COUNT, "ll40ls: comms errors")),
+	_sensor_resets(perf_alloc(PC_COUNT, "ll40ls: resets")),
+	_sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls: zero resets"))
 {
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-			/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-			/* set default polling rate */
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_interval == 0);
-
-					/* set interval for next measurement to minimum legal value */
-					_measure_interval = (LL40LS_CONVERSION_INTERVAL);
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start) {
-						start();
-					}
-
-					return OK;
-				}
-
-			/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_interval == 0);
-
-					/* convert hz to tick interval via microseconds */
-					unsigned interval = (1000000 / arg);
-
-					/* check against maximum rate */
-					if (interval < (LL40LS_CONVERSION_INTERVAL)) {
-						return -EINVAL;
-					}
-
-					/* update interval for next measurement */
-					_measure_interval = interval;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start) {
-						start();
-					}
-
-					return OK;
-				}
-			}
-		}
-
-	case SENSORIOCRESET:
-		reset_sensor();
-		return OK;
+	_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
+	_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V3);
+	_px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian
+}
 
-	default:
-		return -EINVAL;
-	}
+LidarLite::~LidarLite()
+{
+	perf_free(_sample_perf);
+	perf_free(_sample_interval_perf);
+	perf_free(_comms_errors);
+	perf_free(_sensor_resets);
+	perf_free(_sensor_zero_resets);
+};
+
+void
+LidarLite::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_sample_interval_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_sensor_resets);
+	perf_print_counter(_sensor_zero_resets);
+	printf("poll interval:  %u \n", get_measure_interval());
 }

src/drivers/distance_sensor/ll40ls/LidarLite.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,7 +41,8 @@
 #pragma once
 
 #include <drivers/device/device.h>
-#include <drivers/drv_range_finder.h>
+#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
+#include <perf/perf_counter.h>
 
 /* Device limits */
 #define LL40LS_MIN_DISTANCE (0.05f)
@@ -57,51 +58,43 @@
 class LidarLite
 {
 public:
-	LidarLite() = default;
-	virtual ~LidarLite() = default;
+	LidarLite(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	virtual ~LidarLite();
 
 	virtual int init() = 0;
-
-	virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
-
 	virtual void start() = 0;
-
 	virtual void stop() = 0;
 
 	/**
 	* @brief
 	*   Diagnostics - print some basic information about the driver.
 	*/
-	virtual void print_info() = 0;
+	void print_info();
 
 	/**
 	 * @brief
 	 *   print registers to console
 	 */
-	virtual void print_registers() = 0;
-
-	virtual const char *get_dev_name() = 0;
+	virtual void print_registers() {};
 
 protected:
-	/**
-	* Set the min and max distance thresholds if you want the end points of the sensors
-	* range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE
-	* and LL40LS_MAX_DISTANCE_V3
-	*/
-	void                set_minimum_distance(const float min) { _min_distance = min; }
-	void                set_maximum_distance(const float max) { _max_distance = max; }
-	float               get_minimum_distance() const { return _min_distance; }
-	float               get_maximum_distance() const { return _max_distance; }
 
-	uint32_t            getMeasureInterval() const { return _measure_interval; }
+	uint32_t	get_measure_interval() const { return _measure_interval; }
+
+	virtual int	measure() = 0;
+	virtual int	collect() = 0;
 
-	virtual int         measure() = 0;
-	virtual int         collect() = 0;
+	virtual int	reset_sensor() { return PX4_ERROR; };
 
-	virtual int         reset_sensor() = 0;
+	PX4Rangefinder	_px4_rangefinder;
+
+	perf_counter_t	_sample_perf;
+	perf_counter_t	_sample_interval_perf;
+	perf_counter_t	_comms_errors;
+	perf_counter_t	_sensor_resets;
+	perf_counter_t	_sensor_zero_resets;
 
 private:
-	float               _min_distance{LL40LS_MIN_DISTANCE};
-	float               _max_distance{LL40LS_MAX_DISTANCE_V3};
-	uint32_t            _measure_interval{0};
+
+	uint32_t            _measure_interval{LL40LS_CONVERSION_INTERVAL};
 };