ll40ls: add support for LidarLite V4

Co-authored-by: Claudio Micheli <claudio@auterion.com>

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -140,8 +140,35 @@ LidarLiteI2C::probe()
 		 * 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.
 		 */
+
+		if (read_reg(LL40LS_HW_VERSION_V4, _hw_version) == OK  &&
+		    read_reg(LL40LS_SW_VERSION_V4, _sw_version) == OK) {
+			if (_hw_version > 0) {
+				PX4_DEBUG("probe success - hw: %u, sw: %u", _hw_version, _sw_version);
+
+				uint8_t id_byte_0 = 0;
+				uint8_t id_byte_1 = 0;
+				uint8_t id_byte_2 = 0;
+				uint8_t id_byte_3 = 0;
+
+				if (read_reg(LL40LS_UNIT_ID_0_V4, id_byte_0) == OK &&
+				    read_reg(LL40LS_UNIT_ID_1_V4, id_byte_1) == OK &&
+				    read_reg(LL40LS_UNIT_ID_2_V4, id_byte_2) == OK &&
+				    read_reg(LL40LS_UNIT_ID_3_V4, id_byte_3) == OK) {
+					_unit_id = ((uint32_t)id_byte_3 << 24) | ((uint32_t)id_byte_2 << 16) | ((uint32_t)id_byte_1 << 8) |
+						   (uint32_t)id_byte_0;
+				}
+
+				_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V4);
+				_model = Model::v4;
+				return OK;
+			}
+
+		}
+
 		if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK) &&
 		    (read_reg(LL40LS_SW_VERSION, _sw_version) == OK)) {
+			PX4_DEBUG("hw: %u - sw %u \n", _hw_version, _sw_version);
 
 			if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK &&
 			    read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) {
@@ -152,20 +179,20 @@ LidarLiteI2C::probe()
 
 				if (_unit_id > 0) {
 					// v2
-					PX4_INFO("probe success - hw: %" PRIu8 ", sw:%" PRIu8 ", id: %" PRIu16, _hw_version, _sw_version, _unit_id);
+					PX4_DEBUG("probe success - hw: %" PRIu8 ", sw:%" PRIu8 ", id: %" PRIu16, _hw_version, _sw_version, _unit_id);
 					_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V2);
 
 				} else {
 					// v1 and v3
-					PX4_INFO("probe success - hw: %" PRIu8 ", sw:%" PRIu8, _hw_version, _sw_version);
+					PX4_DEBUG("probe success - hw: %" PRIu8 ", sw:%" PRIu8, _hw_version, _sw_version);
 				}
 
 			} else {
 
 				if (_unit_id > 0) {
 					// v3hp
-					_is_v3hp = true;
+					_model = Model::v3hp;
-					PX4_INFO("probe success - id: %" PRIu16, _unit_id);
+					PX4_DEBUG("probe success - id: %" PRIu16, _unit_id);
 				}
 			}
 
@@ -289,18 +316,29 @@ LidarLiteI2C::print_registers()
 int
 LidarLiteI2C::collect()
 {
+
 	// read from the sensor
 	uint8_t val[2] {};
+	int ret = 0;
 
 	perf_begin(_sample_perf);
 
+	uint8_t invalid_distance_index = 0;
+
 	// read the high and low byte distance registers
-	uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
+	if (_model == Model::v4) {
-	int ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
+		uint8_t distance_reg = LL40LS_DISTHIGH_REG_V4;
+		ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
+		invalid_distance_index = 1;
+
+	} else {
+		uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
+		ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
+	}
 
 	// if the transfer failed or if the high bit of distance is
 	// set then the distance is invalid
-	if (ret < 0 || (val[0] & 0x80)) {
+	if (ret < 0 || (val[invalid_distance_index] & 0x80)) {
 		if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
 			/*
 			  NACKs from the sensor are expected when we
@@ -322,7 +360,15 @@ LidarLiteI2C::collect()
 		return ret;
 	}
 
-	uint16_t distance_cm = (val[0] << 8) | val[1];
+	uint16_t distance_cm = 0;
+
+	if (_model == Model::v4) {
+		distance_cm = ((uint16_t)(val[1] << 8)) | (uint16_t)val[0];
+
+	} else {
+		distance_cm = (val[0] << 8) | val[1];
+	}
+
 	const float distance_m = float(distance_cm) * 1e-2f;
 
 	if (distance_cm == 0) {
@@ -349,61 +395,25 @@ LidarLiteI2C::collect()
 	// this should be fairly close to the end of the measurement, so the best approximation of the time
 	const hrt_abstime timestamp_sample = hrt_absolute_time();
 
-	// Relative signal strength measurement, i.e. the strength of
+	int8_t signal_quality = -1;
-	// 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 (_model != Model::v4) {
-	if (ret < 0) {
+		// Relative signal strength measurement, i.e. the strength of
-		if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) {
+		// the main signal peak compared to the general noise level.
-			/*
+		uint8_t signal_strength_reg = LL40LS_SIGNAL_STRENGTH_REG;
-			  NACKs from the sensor are expected when we
+		ret = lidar_transfer(&signal_strength_reg, 1, &val[0], 1);
-			  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];
-	uint8_t signal_quality;
-
-	// We detect if V3HP is being used
-	if (_is_v3hp) {
-		//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.
-		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
+				  NACKs from the sensor are expected when we
-				read before it is ready, so only consider it
+				  read before it is ready, so only consider it
-				an error if more than 100ms has elapsed.
+				  an error if more than 100ms has elapsed.
-				*/
+				 */
-				PX4_INFO("peak strength read failed");
+				PX4_INFO("signal strength read failed");
 
-				DEVICE_DEBUG("error reading peak strength from sensor: %d", ret);
+				DEVICE_DEBUG("error reading signal strength from sensor: %d", ret);
 				perf_count(_comms_errors);
 
 				if (perf_event_count(_comms_errors) % 10 == 0) {
@@ -418,18 +428,58 @@ LidarLiteI2C::collect()
 			return ret;
 		}
 
-		uint8_t ll40ls_peak_strength = val[0];
+		uint8_t ll40ls_signal_strength = val[0];
 
-		// For v2 and v3 use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements
+		// We detect if V3HP is being used
-		if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW || distance_m < LL40LS_MIN_DISTANCE) {
+		if (_model == Model::v3hp) {
-			signal_quality = 0;
+			//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 {
-			//Normalize signal strength to 0...100 percent using the absolute signal peak strength.
+			// Absolute peak strength measurement, i.e. absolute strength of main signal peak.
-			signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW, 0) /
+			uint8_t peak_strength_reg = LL40LS_PEAK_STRENGTH_REG;
-					 (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
+			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();
+					}
+				}
+
+				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];
+
+			// 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 || distance_m < LL40LS_MIN_DISTANCE) {
+				signal_quality = 0;
+
+			} else {
+				//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);
+
+			}
 		}
+
 	}
 
 	_px4_rangefinder.update(timestamp_sample, distance_m, signal_quality);

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -83,6 +83,15 @@ static constexpr int LL40LS_PEAK_STRENGTH_HIGH        = 234; /* Max peak strengt
 static constexpr float LL40LS_MIN_DISTANCE{0.05f};
 static constexpr float LL40LS_MAX_DISTANCE{25.00f};
 static constexpr float LL40LS_MAX_DISTANCE_V2{35.00f};
+static constexpr float LL40LS_MAX_DISTANCE_V4{10.00f};
+
+static constexpr uint8_t LL40LS_HW_VERSION_V4         = 0xE1;
+static constexpr uint8_t LL40LS_SW_VERSION_V4         = 0x30;
+static constexpr uint8_t LL40LS_UNIT_ID_0_V4	      = 0x16;
+static constexpr uint8_t LL40LS_UNIT_ID_1_V4 	      = 0x17;
+static constexpr uint8_t LL40LS_UNIT_ID_2_V4	      = 0x18;
+static constexpr uint8_t LL40LS_UNIT_ID_3_V4	      = 0x19;
+static constexpr uint8_t LL40LS_DISTHIGH_REG_V4       = 0x10; /* High byte of distance register, auto increment */
 
 // Normal conversion wait time.
 static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{50_ms};
@@ -168,13 +177,20 @@ private:
 	int probe_address(const uint8_t address);
 
 	bool _collect_phase{false};
-	bool _is_v3hp{false};
 	bool _pause_measurements{false};
 
+	enum class Model {
+		Generic = 0,
+		v3hp = 1,
+		v4 = 2,
+	};
+
+	Model _model{Model::Generic};
+
 	uint8_t _hw_version{0};
 	uint8_t _sw_version{0};
 
-	uint16_t _unit_id{0};
+	uint32_t _unit_id{0};
 	uint16_t _zero_counter{0};
 
 	uint64_t _acquire_time_usec{0};