ll40ls driver: support also lidar lite v3HP (#10578)

src/drivers/distance_sensor/ll40ls/LidarLite.cpp

@@ -43,7 +43,7 @@
 
 LidarLite::LidarLite() :
 	_min_distance(LL40LS_MIN_DISTANCE),
-	_max_distance(LL40LS_MAX_DISTANCE),
+	_max_distance(LL40LS_MAX_DISTANCE_V3),
 	_measure_ticks(0)
 {
 }

src/drivers/distance_sensor/ll40ls/LidarLite.h

@@ -45,7 +45,8 @@
 
 /* Device limits */
 #define LL40LS_MIN_DISTANCE (0.05f)
-#define LL40LS_MAX_DISTANCE (25.00f)
+#define LL40LS_MAX_DISTANCE_V3 (35.00f)
+#define LL40LS_MAX_DISTANCE_V1 (25.00f)
 
 // normal conversion wait time
 #define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */
@@ -86,7 +87,7 @@ protected:
 	/**
 	* Set the min and max distance thresholds if you want the end points of the sensors
 	* range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE
-	* and LL40LS_MAX_DISTANCE
+	* and LL40LS_MAX_DISTANCE_V3
 	*/
 	void                set_minimum_distance(const float min);
 	void                set_maximum_distance(const float max);

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -69,7 +69,8 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr
 	_acquire_time_usec(0),
 	_pause_measurements(false),
 	_hw_version(0),
-	_sw_version(0)
+	_sw_version(0),
+	_unit_id(0)
 {
 	// up the retries since the device misses the first measure attempts
 	_retries = 3;
@@ -88,6 +89,10 @@ LidarLiteI2C::~LidarLiteI2C()
 		delete _reports;
 	}
 
+	if (_distance_sensor_topic != nullptr) {
+		orb_unadvertise(_distance_sensor_topic);
+	}
+
 	if (_class_instance != -1) {
 		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
 	}
@@ -172,22 +177,46 @@ int LidarLiteI2C::probe()
 	// cope with both old and new I2C bus address
 	const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
 
+	uint8_t id_high = 0, id_low = 0;
+
 	// more retries for detection
 	_retries = 10;
 
 	for (uint8_t i = 0; i < sizeof(addresses); i++) {
-		/*
+
-		  check for hw and sw versions. It would be better if
+		set_device_address(addresses[i]);
-		  we had a proper WHOAMI register
+
-		 */
+		if (addresses[i] == LL40LS_BASEADDR) {
-		if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 &&
+
-		    read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) {
+			/*
-			goto 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 && id_high > 0 &&
+			    read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK && id_low > 0) {
+				_unit_id = (uint16_t)((id_high << 8) | id_low) & 0xFFFF;
+				goto ok;
+			}
+
+			PX4_DEBUG("probe failed unit_id=0x%02x\n",
+				  (unsigned)_unit_id);
+
+		} 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) {
+				set_maximum_distance(LL40LS_MAX_DISTANCE_V1);
+				goto ok;
+			}
+
+			PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n",
+				  (unsigned)_hw_version,
+				  (unsigned)_sw_version);
 		}
 
-		PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n",
-			  (unsigned)_hw_version,
-			  (unsigned)_sw_version);
 	}
 
 	// not found on any address

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -68,6 +68,8 @@
 #define LL40LS_SW_VERSION         0x4f
 #define LL40LS_SIGNAL_STRENGTH_REG  0x0e
 #define LL40LS_PEAK_STRENGTH_REG  0x0c
+#define LL40LS_UNIT_ID_HIGH 0x16
+#define LL40LS_UNIT_ID_LOW 0x17
 
 #define LL40LS_SIG_COUNT_VAL_REG      0x02        /* Maximum acquisition count register */
 #define LL40LS_SIG_COUNT_VAL_MAX     0xFF        /* Maximum acquisition count max value */
@@ -130,6 +132,7 @@ private:
 	volatile bool       _pause_measurements;
 	uint8_t		_hw_version;
 	uint8_t		_sw_version;
+	uint16_t	_unit_id;
 
 	/**
 	 * LidarLite specific transfer function. This is needed

src/drivers/distance_sensor/ll40ls/ll40ls.cpp

@@ -114,6 +114,7 @@ void start(enum LL40LS_BUS busid, uint8_t rotation)
 
 	if (instance) {
 		PX4_INFO("driver already started");
+		return;
 	}
 
 	if (busid == LL40LS_BUS_PWM) {
@@ -149,7 +150,6 @@ void start(enum LL40LS_BUS busid, uint8_t rotation)
 
 			PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum);
 			stop();
-			return;
 		}
 	}