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

msg/distance_sensor.msg

@@ -5,6 +5,7 @@ float32 min_distance		# Minimum distance the sensor can measure (in m)
 float32 max_distance		# Maximum distance the sensor can measure (in m)
 float32 current_distance	# Current distance reading (in m)
 float32 covariance		# Measurement covariance (in m), 0 for unknown / invalid readings
+int8 signal_strength		# Signal strength in percent (0...100%) or -1 if unknown
 
 uint8 type			# Type from MAV_DISTANCE_SENSOR enum
 uint8 MAV_DISTANCE_SENSOR_LASER = 0

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.signal_strength = signal_strength;
-	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	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;	// TODO: set proper ID
-	report.id = 0;
 
 	/* 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;

src/modules/ekf2/ekf2_main.cpp

@@ -941,7 +941,8 @@ void Ekf2::run()
 				if (orb_copy(ORB_ID(distance_sensor), _range_finder_subs[_range_finder_sub_index], &range_finder) == PX4_OK) {
 					// check if distance sensor is within working boundaries
 					if (range_finder.min_distance >= range_finder.current_distance ||
-					    range_finder.max_distance <= range_finder.current_distance) {
+					    range_finder.max_distance <= range_finder.current_distance ||
+					    range_finder.signal_strength == 0) {
 						// use rng_gnd_clearance if on ground
 						if (_ekf.get_in_air_status()) {
 							range_finder_updated = false;
@@ -951,7 +952,7 @@ void Ekf2::run()
 						}
 					}
 
-					_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
+					if (range_finder_updated) { _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance); }
 
 					// Save sensor limits reported by the rangefinder
 					_ekf.set_rangefinder_limits(range_finder.min_distance, range_finder.max_distance);

src/modules/simulator/simulator_mavlink.cpp

@@ -1173,6 +1173,7 @@ int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink)
 	dist.id = dist_mavlink->id;
 	dist.orientation = dist_mavlink->orientation;
 	dist.covariance = dist_mavlink->covariance / 100.0f;
+	dist.signal_strength = -1;
 
 	int dist_multi;
 	orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH);

src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp

@@ -162,6 +162,8 @@ int DfBebopRangeFinderWrapper::_publish(struct bebop_range &data)
 
 	distance_data.covariance = 1.0f; // TODO set correct value
 
+	distance_data.signal_strength = -1;
+
 	if (_range_topic == nullptr) {
 		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &distance_data,
 						   &_orb_class_instance, ORB_PRIO_DEFAULT);

src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp

@@ -169,6 +169,8 @@ int DfISL29501Wrapper::_publish(struct range_sensor_data &data)
 
 	d.covariance = 0.0f;
 
+	d.signal_strength = -1;
+
 	if (_range_topic == nullptr) {
 		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
 						   &_orb_class_instance, ORB_PRIO_DEFAULT);

src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp

@@ -175,6 +175,8 @@ int DfTROneWrapper::_publish(struct range_sensor_data &data)
 
 	d.covariance = 0.0f;
 
+	d.signal_strength = -1;
+
 	if (_range_topic == nullptr) {
 		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
 						   &_orb_class_instance, ORB_PRIO_DEFAULT);