[WIP] Lidar Lite multi instance

src/drivers/distance_sensor/ll40ls/LidarLite.cpp

@@ -45,7 +45,7 @@ LidarLite::LidarLite(uint8_t rotation) :
 	_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation)
 {
 	_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
-	_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V3);
+	_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE);
 	_px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian
 }
 

src/drivers/distance_sensor/ll40ls/LidarLite.h

@@ -46,8 +46,8 @@
 
 /* Device limits */
 #define LL40LS_MIN_DISTANCE (0.05f)
-#define LL40LS_MAX_DISTANCE_V3 (35.00f)
-#define LL40LS_MAX_DISTANCE_V1 (25.00f)
+#define LL40LS_MAX_DISTANCE (35.00f)
+
 
 // normal conversion wait time
 #define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -128,35 +128,42 @@ LidarLiteI2C::probe()
 
 		set_device_address(addresses[i]);
 
-		if (addresses[i] == LL40LS_BASEADDR) {
-
-			/*
-			  check for unit id. It would be better if
-			  we had a proper WHOAMI register
-			 */
-			if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK &&
-			    read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) {
-				_unit_id = (uint16_t)((id_high << 8) | id_low) & 0xFFFF;
-				_retries = 3;
-				return reset_sensor();
-			}
+		/*
+		The probing is divided in two cases. One to detect v1, v2 and v3 and one to
+		detect v3HP. The reason for this is that registers are not consistent
+		between different versions. The v3HP doesn't have the HW VERSION (or at least no version is specified)
+		while v1, v2 and some v3 don't have the unit id register.
+		It would be better if we had a proper WHOAMI register.
+		*/
+
+
+		/*
+		  check for hw and sw versions for v1, v2 and v3
+		 */
+		if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version) > 0 &&
+		    (read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0)) {
+			PX4_INFO("probe success - hw: %u, sw:%u", _hw_version, _sw_version);
+			_retries = 3;
+			return reset_sensor();
+		}
 
-			PX4_DEBUG("probe failed unit_id=0x%02x", (unsigned)_unit_id);
+		/*
+		  check for unit id for v3HP
+		 */
+		if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK &&
+		    read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) {
+			_unit_id = (uint16_t)((id_high << 8) | id_low) & 0xFFFF;
+			_is_V3hp = true;
+			PX4_INFO("probe success - id: %u", _unit_id);
+			_retries = 3;
+			return reset_sensor();
+		}
 
-		} else {
-			/*
-			  check for hw and sw versions. It would be better if
-			  we had a proper WHOAMI register
-			 */
-			if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 &&
-			    read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) {
-				_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V1);
-				_retries = 3;
-				return reset_sensor();
-			}
+		PX4_DEBUG("probe failed unit_id=0x%02x hw_version=0x%02x sw_version=0x%02x",
+			  (unsigned)_unit_id,
+			  (unsigned)_hw_version,
+			  (unsigned)_sw_version);
 
-			PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x", (unsigned)_hw_version, (unsigned)_sw_version);
-		}
 	}
 
 	// not found on any address
@@ -205,12 +212,33 @@ LidarLiteI2C::measure()
 int
 LidarLiteI2C::reset_sensor()
 {
-	int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
+	int ret;
+
+	px4_usleep(15000);
+
+	ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX);
 
 	if (ret != OK) {
 		return ret;
 	}
 
+	px4_usleep(15000);
+	ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
+
+
+	if (ret != OK) {
+		uint8_t sig_cnt;
+
+		px4_usleep(15000);
+		ret = read_reg(LL40LS_SIG_COUNT_VAL_REG, sig_cnt);
+
+		if ((ret != OK) || (sig_cnt != LL40LS_SIG_COUNT_VAL_DEFAULT)) {
+			PX4_INFO("Error: ll40ls reset failure. Exiting!\n");
+			return ret;
+
+		}
+	}
+
 	// wait for sensor reset to complete
 	px4_usleep(50000);
 	ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX);
@@ -349,49 +377,65 @@ int LidarLiteI2C::collect()
 
 	uint8_t ll40ls_signal_strength = val[0];
 
+	uint8_t signal_min = 0;
+	uint8_t signal_max = 0;
+	uint8_t signal_value = 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);
+	// We detect if V3HP is being used
+	if (_is_V3hp) {
+		signal_min = LL40LS_SIGNAL_STRENGH_MIN_V3HP;
+		signal_max = LL40LS_SIGNAL_STRENGH_MAX_V3HP;
+		signal_value = ll40ls_signal_strength;
 
-	// check if the transfer failed
-	if (ret < 0) {
-		if (hrt_elapsed_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");
+	} else {
+		/* 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_elapsed_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();
+				}
+			}
 
-			DEVICE_DEBUG("error reading peak strength from sensor: %d", ret);
-			perf_count(_comms_errors);
+			perf_end(_sample_perf);
+			// if we are getting lots of I2C transfer errors try
+			// resetting the sensor
+			return ret;
+		}
 
-			if (perf_event_count(_comms_errors) % 10 == 0) {
-				perf_count(_sensor_resets);
-				reset_sensor();
-			}
+		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;
+
+		} else {
+			signal_value = ll40ls_peak_strength;
 		}
 
-		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_quality = 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_quality = 0;
-	}
+	uint8_t signal_quality = 100 * math::max(signal_value - signal_min, 0) / (signal_max - signal_min);
 
-	// Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
+	// Step 2: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
 	if (distance_m < LL40LS_MIN_DISTANCE) {
 		signal_quality = 0;
 	}

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -49,6 +49,7 @@
 /* Configuration Constants */
 static constexpr uint8_t LL40LS_BASEADDR	= 0x62;	/* 7-bit address */
 static constexpr uint8_t LL40LS_BASEADDR_OLD	= 0x42;	/* previous 7-bit address */
+static constexpr uint8_t LL40LS_SIG_COUNT_VAL_DEFAULT = 0x80; /* Default maximum acquisition count */
 
 /* LL40LS Registers addresses */
 static constexpr uint8_t LL40LS_MEASURE_REG	= 0x00;	/* Measure range register */
@@ -66,6 +67,8 @@ static constexpr uint8_t LL40LS_UNIT_ID_LOW	= 0x17;
 
 static constexpr uint8_t LL40LS_SIG_COUNT_VAL_REG	= 0x02;	/* Maximum acquisition count register */
 static constexpr uint8_t LL40LS_SIG_COUNT_VAL_MAX	= 0xFF;	/* Maximum acquisition count max value */
+static constexpr int LL40LS_SIGNAL_STRENGH_MIN_V3HP = 70;	// Min signal strength for V3HP
+static constexpr int LL40LS_SIGNAL_STRENGH_MAX_V3HP = 255;	// Max signal strength for V3HP
 
 static constexpr int LL40LS_SIGNAL_STRENGTH_LOW =
 	24;	// Minimum (relative) signal strength value for accepting a measurement
@@ -147,5 +150,6 @@ class LidarLiteI2C : public LidarLite, public device::I2C, public px4::Scheduled
 	uint8_t		_hw_version{0};
 	uint8_t		_sw_version{0};
 	uint16_t	_unit_id{0};
+	bool 		_is_V3hp{false};
 
 };