LL40LS: Implement driver-specific filtering based on the datasheet and experiments

src/drivers/distance_sensor/leddar_one/leddar_one.cpp

@@ -411,6 +411,7 @@ void leddar_one::_publish(uint16_t distance_mm)
 	report.min_distance = MIN_DISTANCE;
 	report.max_distance = MAX_DISTANCE;
 	report.covariance = 0.0f;
+	report.signal_strength = -1;
 	report.id = 0;
 
 	_reports->force(&report);

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -47,6 +47,7 @@
 #include <poll.h>
 #include <string.h>
 #include <stdio.h>
+#include <mathlib/mathlib.h>
 #include <drivers/drv_hrt.h>
 
 LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int address) :
@@ -448,6 +449,80 @@ int LidarLiteI2C::collect()
 
 	_last_distance = distance_cm;
 
+	/* Relative signal strength measurement, i.e. the strength of
+	 * the main signal peak compared to the general noise level*/
+	uint8_t signal_strength_reg = LL40LS_SIGNAL_STRENGTH_REG;
+	ret = lidar_transfer(&signal_strength_reg, 1, &val[0], 1);
+
+	// check if the transfer failed
+	if (ret < 0) {
+		if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
+			/*
+			  NACKs from the sensor are expected when we
+			  read before it is ready, so only consider it
+			  an error if more than 100ms has elapsed.
+			 */
+			PX4_INFO("signal strength read failed");
+
+			DEVICE_DEBUG("error reading signal strength from sensor: %d", ret);
+			perf_count(_comms_errors);
+
+			if (perf_event_count(_comms_errors) % 10 == 0) {
+				perf_count(_sensor_resets);
+				reset_sensor();
+			}
+		}
+
+		perf_end(_sample_perf);
+		// if we are getting lots of I2C transfer errors try
+		// resetting the sensor
+		return ret;
+	}
+
+	uint8_t ll40ls_signal_strength = val[0];
+
+
+	/* Absolute peak strength measurement, i.e. absolute strength of main signal peak*/
+	uint8_t peak_strength_reg = LL40LS_PEAK_STRENGTH_REG;
+	ret = lidar_transfer(&peak_strength_reg, 1, &val[0], 1);
+
+	// check if the transfer failed
+	if (ret < 0) {
+		if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
+			/*
+			  NACKs from the sensor are expected when we
+			  read before it is ready, so only consider it
+			  an error if more than 100ms has elapsed.
+			 */
+			PX4_INFO("peak strenght read failed");
+
+			DEVICE_DEBUG("error reading peak strength from sensor: %d", ret);
+			perf_count(_comms_errors);
+
+			if (perf_event_count(_comms_errors) % 10 == 0) {
+				perf_count(_sensor_resets);
+				reset_sensor();
+			}
+		}
+
+		perf_end(_sample_perf);
+		// if we are getting lots of I2C transfer errors try
+		// resetting the sensor
+		return ret;
+	}
+
+	uint8_t ll40ls_peak_strength = val[0];
+
+	/* Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments*/
+	// Step 1: Normalize signal strength to 0...100 percent using the absolute signal peak strength.
+	uint8_t signal_strength = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW,
+				  0) / (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
+
+	// Step 2: Also use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements
+	if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW) { signal_strength = 0; }
+
+	// Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
+	if (distance_m < LL40LS_MIN_DISTANCE) { signal_strength = 0; }
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
@@ -455,11 +530,11 @@ int LidarLiteI2C::collect()
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
 	report.covariance = 0.0f;
-	/* the sensor is in fact a laser + sonar but there is no enum for this */
-	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.signal_strength = signal_strength;
+	report.type =
+		distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;		// the sensor is in fact a laser + sonar but there is no enum for this
 	report.orientation = _rotation;
-	/* TODO: set proper ID */
-	report.id = 0;
+	report.id = 0;	// TODO: set proper ID
 
 	/* publish it, if we are the primary */
 	if (_distance_sensor_topic != nullptr) {

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -66,11 +66,16 @@
 #define LL40LS_AUTO_INCREMENT   0x80
 #define LL40LS_HW_VERSION         0x41
 #define LL40LS_SW_VERSION         0x4f
-#define LL40LS_SIGNAL_STRENGTH_REG  0x5b
+#define LL40LS_SIGNAL_STRENGTH_REG  0x0e
+#define LL40LS_PEAK_STRENGTH_REG  0x0c
 
 #define LL40LS_SIG_COUNT_VAL_REG      0x02        /* Maximum acquisition count register */
 #define LL40LS_SIG_COUNT_VAL_MAX     0xFF        /* Maximum acquisition count max value */
 
+#define LL40LS_SIGNAL_STRENGTH_LOW 24			// Minimum (relative) signal strength value for accepting a measurement
+#define LL40LS_PEAK_STRENGTH_LOW 135			// Minimum peak strength raw value for accepting a measurement
+#define LL40LS_PEAK_STRENGTH_HIGH 234			// Max peak strength raw value
+
 class LidarLiteI2C : public LidarLite, public device::I2C
 {
 public:

src/drivers/distance_sensor/mb12xx/mb12xx.cpp

@@ -556,6 +556,7 @@ MB12XX::collect()
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
 	report.covariance = 0.0f;
+	report.signal_strength = -1;
 	/* TODO: set proper ID */
 	report.id = 0;
 

src/drivers/distance_sensor/sf0x/sf0x.cpp

@@ -616,6 +616,7 @@ SF0X::collect()
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
 	report.covariance = 0.0f;
+	report.signal_strength = -1;
 	/* TODO: set proper ID */
 	report.id = 0;
 

src/drivers/distance_sensor/sf1xx/sf1xx.cpp

@@ -552,6 +552,7 @@ SF1XX::collect()
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
 	report.covariance = 0.0f;
+	report.signal_strength = -1;
 	/* TODO: set proper ID */
 	report.id = 0;
 

src/drivers/distance_sensor/srf02/srf02.cpp

@@ -559,6 +559,7 @@ SRF02::collect()
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
 	report.covariance = 0.0f;
+	report.signal_strength = -1;
 	/* TODO: set proper ID */
 	report.id = 0;
 

src/drivers/distance_sensor/teraranger/teraranger.cpp

@@ -633,6 +633,7 @@ TERARANGER::collect()
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
 	report.covariance = 0.0f;
+	report.signal_strength = -1;
 	/* TODO: set proper ID */
 	report.id = 0;
 

src/drivers/distance_sensor/tfmini/tfmini.cpp

@@ -580,6 +580,7 @@ TFMINI::collect()
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
 	report.covariance = 0.0f;
+	report.signal_strength = -1;
 	/* TODO: set proper ID */
 	report.id = 0;
 

src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp

@@ -675,6 +675,7 @@ VL53LXX::collect()
 	report.min_distance = VL53LXX_MIN_RANGING_DISTANCE;
 	report.max_distance = VL53LXX_MAX_RANGING_DISTANCE;
 	report.covariance = 0.0f;
+	report.signal_strength = -1;
 
 	/* TODO: set proper ID */
 	report.id = 0;

src/drivers/px4flow/px4flow.cpp

@@ -595,6 +595,7 @@ PX4FLOW::collect()
 	distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
 	distance_report.current_distance = report.ground_distance_m;
 	distance_report.covariance = 0.0f;
+	distance_report.signal_strength = -1;
 	distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
 	/* TODO: the ID needs to be properly set */
 	distance_report.id = 0;