diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp
index fb13203516..9a05890cd0 100644
--- a/src/drivers/ll40ls/LidarLiteI2C.cpp
+++ b/src/drivers/ll40ls/LidarLiteI2C.cpp
@@ -72,6 +72,8 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
 	_zero_counter(0),
 	_acquire_time_usec(0),
 	_pause_measurements(false),
+	_hw_version(0),
+	_sw_version(0),
 	_bus(bus)
 {
 	// up the retries since the device misses the first measure attempts
@@ -146,7 +148,30 @@ out:
 
 int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val)
 {
-	return transfer(&reg, 1, &val, 1);
+	return lidar_transfer(&reg, 1, &val, 1);
+}
+
+int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val)
+{
+	const uint8_t cmd[2] = { reg, val };
+	return transfer(&cmd[0], 2, NULL, 0);
+}
+
+/*
+  LidarLite specific transfer() function that avoids a stop condition
+  with SCL low
+ */
+int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	if (send != NULL && send_len > 0) {
+		int ret = transfer(send, send_len, NULL, 0);
+		if (ret != OK)
+			return ret;
+	}
+	if (recv != NULL && recv_len > 0) {
+		return transfer(NULL, 0, recv, recv_len);
+	}
+	return OK;
 }
 
 int LidarLiteI2C::probe()
@@ -158,28 +183,18 @@ int LidarLiteI2C::probe()
 	_retries = 10;
 
 	for (uint8_t i = 0; i < sizeof(addresses); i++) {
-		uint8_t who_am_i = 0, max_acq_count = 0;
-
-		// set the I2C bus address
-		set_address(addresses[i]);
-
-		/* register 2 defaults to 0x80. If this matches it is
-		   almost certainly a ll40ls */
-		if (read_reg(LL40LS_MAX_ACQ_COUNT_REG, max_acq_count) == OK && max_acq_count == 0x80) {
-			// very likely to be a ll40ls. This is the
-			// default max acquisition counter
+		/*
+		  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) {
 			goto ok;
 		}
 
-		if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) {
-			// it is responding correctly to a
-			// WHO_AM_I. This works with older sensors (pre-production)
-			goto ok;
-		}
-
-		DEVICE_DEBUG("probe failed reg11=0x%02x reg2=0x%02x\n",
-		      (unsigned)who_am_i,
-		      (unsigned)max_acq_count);
+		DEVICE_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n",
+			     (unsigned)_hw_version,
+			     (unsigned)_sw_version);
 	}
 
 	// not found on any address
@@ -187,9 +202,6 @@ int LidarLiteI2C::probe()
 
 ok:
 	_retries = 3;
-
-	// reset the sensor to ensure it is in a known state with
-	// correct settings
 	return reset_sensor();
 }
 
@@ -303,7 +315,7 @@ int LidarLiteI2C::measure()
 	 * Send the command to begin a measurement.
 	 */
 	const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
-	ret = transfer(cmd, sizeof(cmd), nullptr, 0);
+	ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0);
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
@@ -332,9 +344,14 @@ int LidarLiteI2C::measure()
  */
 int LidarLiteI2C::reset_sensor()
 {
-	const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET };
-	int ret = transfer(cmd, sizeof(cmd), nullptr, 0);
-	return ret;
+	int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
+	if (ret != OK)
+		return ret;
+
+	// wait for sensor reset to complete
+	usleep(1000);
+
+	return OK;
 }
 
 /*
@@ -349,7 +366,7 @@ void LidarLiteI2C::print_registers()
 
 	for (uint8_t reg = 0; reg <= 0x67; reg++) {
 		uint8_t val = 0;
-		int ret = transfer(&reg, 1, &val, 1);
+		int ret = lidar_transfer(&reg, 1, &val, 1);
 
 		if (ret != OK) {
 			printf("%02x:XX ", (unsigned)reg);
@@ -377,10 +394,12 @@ int LidarLiteI2C::collect()
 	perf_begin(_sample_perf);
 
 	// read the high and low byte distance registers
-	uint8_t distance_reg = LL40LS_DISTHIGH_REG;
-	ret = transfer(&distance_reg, 1, &val[0], sizeof(val));
+	uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
+	ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
 
-	if (ret < 0) {
+	// 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 (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
 			/*
 			  NACKs from the sensor are expected when we
@@ -427,7 +446,8 @@ int LidarLiteI2C::collect()
 		_zero_counter = 0;
 	}
 
-	_last_distance = distance_m;
+	_last_distance = distance_cm;
+	
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h
index d9218e3435..51cca0862f 100644
--- a/src/drivers/ll40ls/LidarLiteI2C.h
+++ b/src/drivers/ll40ls/LidarLiteI2C.h
@@ -63,10 +63,10 @@
 #define LL40LS_MEASURE_REG      0x00        /* Measure range register */
 #define LL40LS_MSRREG_RESET     0x00        /* reset to power on defaults */
 #define LL40LS_MSRREG_ACQUIRE       0x04        /* Value to initiate a measurement, varies based on sensor revision */
-#define LL40LS_MAX_ACQ_COUNT_REG       0x02     /* maximum acquisition count register */
-#define LL40LS_DISTHIGH_REG     0x8F        /* High byte of distance register, auto increment */
-#define LL40LS_WHO_AM_I_REG         0x11
-#define LL40LS_WHO_AM_I_REG_VAL         0xCA
+#define LL40LS_DISTHIGH_REG     0x0F        /* High byte of distance register, auto increment */
+#define LL40LS_AUTO_INCREMENT   0x80
+#define LL40LS_HW_VERSION         0x41
+#define LL40LS_SW_VERSION         0x4f
 #define LL40LS_SIGNAL_STRENGTH_REG  0x5b
 
 class LidarLiteI2C : public LidarLite, public device::I2C
@@ -93,6 +93,7 @@ public:
 protected:
 	int         probe();
 	int         read_reg(uint8_t reg, uint8_t &val);
+	int         write_reg(uint8_t reg, uint8_t val);
 
 	int                 measure() override;
 	int                 reset_sensor();
@@ -116,10 +117,25 @@ private:
 	uint16_t        _zero_counter;
 	uint64_t        _acquire_time_usec;
 	volatile bool       _pause_measurements;
+        uint8_t		_hw_version;
+        uint8_t		_sw_version;
 
 	/**< the bus the device is connected to */
 	int         _bus;
 
+	/**
+	 * LidarLite specific transfer function. This is needed
+	 * to avoid a stop transition with SCL high
+	 *
+	 * @param send		Pointer to bytes to send.
+	 * @param send_len	Number of bytes to send.
+	 * @param recv		Pointer to buffer for bytes received.
+	 * @param recv_len	Number of bytes to receive.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+
 	/**
 	* Test whether the device supported by the driver is present at a
 	* specific address.