From 8a3a71caeddddf46c744487f00a9015e7adb3e24 Mon Sep 17 00:00:00 2001
From: bazooka joe <bazookajoe1900@gmail.com>
Date: Thu, 18 Jun 2020 01:11:40 -0700
Subject: [PATCH] simplify the code to calculate signal_quality

---
 .../distance_sensor/ll40ls/LidarLiteI2C.cpp   | 47 +++++++------------
 1 file changed, 16 insertions(+), 31 deletions(-)

diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
index 2a073fcce7..3b2cbacf14 100644
--- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
@@ -219,7 +219,7 @@ LidarLiteI2C::measure()
 int
 LidarLiteI2C::reset_sensor()
 {
-	px4_usleep(15000);
+	px4_usleep(15_ms);
 
 	int ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX);
 
@@ -227,14 +227,14 @@ LidarLiteI2C::reset_sensor()
 		return ret;
 	}
 
-	px4_usleep(15000);
+	px4_usleep(15_ms);
 	ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
 
 
 	if (ret != PX4_OK) {
 		uint8_t sig_cnt;
 
-		px4_usleep(15000);
+		px4_usleep(15_ms);
 		ret = read_reg(LL40LS_SIG_COUNT_VAL_REG, sig_cnt);
 
 		if ((ret != PX4_OK) || (sig_cnt != LL40LS_SIG_COUNT_VAL_DEFAULT)) {
@@ -245,7 +245,7 @@ LidarLiteI2C::reset_sensor()
 	}
 
 	// wait for sensor reset to complete
-	px4_usleep(50000);
+	px4_usleep(50_ms);
 	ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX);
 
 	if (ret != PX4_OK) {
@@ -253,7 +253,7 @@ LidarLiteI2C::reset_sensor()
 	}
 
 	// wait for register write to complete
-	px4_usleep(1000);
+	px4_usleep(1_ms);
 
 	return OK;
 }
@@ -264,7 +264,7 @@ LidarLiteI2C::print_registers()
 	_pause_measurements = true;
 	PX4_INFO("registers");
 	// wait for a while to ensure the lidar is in a ready state
-	px4_usleep(50000);
+	px4_usleep(50_ms);
 
 	for (uint8_t reg = 0; reg <= 0x67; reg++) {
 		uint8_t val = 0;
@@ -380,16 +380,13 @@ LidarLiteI2C::collect()
 	}
 
 	uint8_t ll40ls_signal_strength = val[0];
-
-	uint8_t signal_min = 0;
-	uint8_t signal_max = 0;
-	uint8_t signal_value = 0;
+	uint8_t signal_quality;
 
 	// We detect if V3HP is being used
 	if (_is_v3hp) {
-		signal_min = LL40LS_SIGNAL_STRENGTH_MIN_V3HP;
-		signal_max = LL40LS_SIGNAL_STRENGTH_MAX_V3HP;
-		signal_value = ll40ls_signal_strength;
+		//Normalize signal strength to 0...100 percent using the absolute signal strength.
+		signal_quality = 100 * math::max(ll40ls_signal_strength - LL40LS_SIGNAL_STRENGTH_MIN_V3HP, 0) /
+				 (LL40LS_SIGNAL_STRENGTH_MAX_V3HP - LL40LS_SIGNAL_STRENGTH_MIN_V3HP);
 
 	} else {
 		// Absolute peak strength measurement, i.e. absolute strength of main signal peak.
@@ -422,31 +419,19 @@ LidarLiteI2C::collect()
 		}
 
 		uint8_t ll40ls_peak_strength = val[0];
-		signal_min = LL40LS_PEAK_STRENGTH_LOW;
-		signal_max = LL40LS_PEAK_STRENGTH_HIGH;
 
 		// For v2 and v3 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_value = 0;
+		if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW || distance_m < LL40LS_MIN_DISTANCE) {
+			signal_quality = 0;
 
 		} else {
-			signal_value = ll40ls_peak_strength;
+			//Normalize signal strength to 0...100 percent using the absolute signal peak strength.
+			signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW, 0) /
+					 (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
+
 		}
 	}
 
-	uint8_t signal_quality;
-
-	if (!_is_v3hp && distance_m < LL40LS_MIN_DISTANCE) {
-		//for Lidar lites that are not v3hp we need to filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
-		signal_quality = 0;
-
-	} else {
-		//Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments
-		//Normalize signal strength to 0...100 percent using the absolute signal peak strength.
-		signal_quality = 100 * math::max(signal_value - signal_min, 0) / (signal_max - signal_min);
-
-	}
-
 	_px4_rangefinder.update(timestamp_sample, distance_m, signal_quality);
 
 	perf_end(_sample_perf);