LidarLite driver fix

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,9 @@
 
 /* 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 (25.00f)
+#define LL40LS_MAX_DISTANCE_V2 (35.00f)
+
 
 // normal conversion wait time
 #define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -128,35 +128,56 @@ LidarLiteI2C::probe()
 
 		set_device_address(addresses[i]);
 
-		if (addresses[i] == LL40LS_BASEADDR) {
+		/**
+		 * The probing is divided into different cases. One to detect v2, one for v3 and v1 and one for 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),
+		 * v1 and v3 don't have the unit id register while v2 has both.
+		 * 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) &&
+		    (read_reg(LL40LS_SW_VERSION, _sw_version) == OK)) {
 
-			/*
-			  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();
 			}
 
-			PX4_DEBUG("probe failed unit_id=0x%02x", (unsigned)_unit_id);
+			if (_hw_version > 0) {
 
-		} 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();
+				if (_unit_id > 0) {
+					// v2
+					PX4_INFO("probe success - hw: %u, sw:%u, id: %u", _hw_version, _sw_version, _unit_id);
+					_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V2);
+
+				} else {
+					// v1 and v3
+					PX4_INFO("probe success - hw: %u, sw:%u", _hw_version, _sw_version);
+				}
+
+			} else {
+
+				if (_unit_id > 0) {
+					// v3hp
+					_is_V3hp = true;
+					PX4_INFO("probe success - id: %u", _unit_id);
+				}
 			}
 
-			PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x", (unsigned)_hw_version, (unsigned)_sw_version);
+			_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);
+
 	}
 
 	// not found on any address
@@ -205,12 +226,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 +391,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_STRENGTH_MIN_V3HP;
+		signal_max = LL40LS_SIGNAL_STRENGTH_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 strength 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_STRENGTH_MIN_V3HP = 70;	// Min signal strength for V3HP
+static constexpr int LL40LS_SIGNAL_STRENGTH_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};
 
 };

src/drivers/distance_sensor/ll40ls/ll40ls.cpp

@@ -89,7 +89,7 @@ namespace ll40ls
 {
 
 LidarLite *instance = nullptr;
-
+int     start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation);
 void    start(enum LL40LS_BUS busid, uint8_t rotation);
 void    stop();
 void    info();
@@ -126,35 +126,53 @@ void start(enum LL40LS_BUS busid, uint8_t rotation)
 				continue;
 			}
 
-			instance = new LidarLiteI2C(bus_options[i].busnum, rotation);
-
-			if (!instance) {
-				PX4_ERR("Failed to instantiate LidarLiteI2C");
+			if (start_bus(bus_options[i], rotation) == PX4_OK) {
 				return;
 			}
 
-			if (instance->init() == PX4_OK) {
-				break;
-			}
-
-			PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum);
-			stop();
 		}
 	}
 
-	if (!instance) {
+	if (instance == nullptr) {
 		PX4_WARN("No LidarLite found");
 		return;
 	}
 }
 
+/**
+ * Start the driver on a specific bus.
+ *
+ * This function call only returns once the driver is up and running
+ * or failed to detect the sensor.
+ */
+int
+start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation)
+{
+	instance = new LidarLiteI2C(i2c_bus.busnum, rotation);
+
+	if (instance->init() != OK) {
+		stop();
+		PX4_INFO("LidarLiteI2C - no device on bus %u", (unsigned)i2c_bus.busid);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
 /**
  * @brief Stops the driver
  */
 void stop()
 {
-	delete instance;
-	instance = nullptr;
+	if (instance != nullptr) {
+		delete instance;
+		instance = nullptr;
+
+	} else {
+		PX4_ERR("driver not running");
+	}
+
+
 }
 
 /**