diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp
index 4c34077e9e..f8d06c865d 100644
--- a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp
+++ b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp
@@ -42,12 +42,7 @@
 #include "LidarLite.h"
 
 LidarLite::LidarLite(uint8_t rotation) :
-	_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls: read")),
-	_sample_interval_perf(perf_alloc(PC_ELAPSED, "ll40ls: interval")),
-	_comms_errors(perf_alloc(PC_COUNT, "ll40ls: comms errors")),
-	_sensor_resets(perf_alloc(PC_COUNT, "ll40ls: resets")),
-	_sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls: zero resets"))
+	_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);
diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.h b/src/drivers/distance_sensor/ll40ls/LidarLite.h
index a958f6ea95..ad57c3d268 100644
--- a/src/drivers/distance_sensor/ll40ls/LidarLite.h
+++ b/src/drivers/distance_sensor/ll40ls/LidarLite.h
@@ -66,35 +66,34 @@ public:
 	virtual void stop() = 0;
 
 	/**
-	* @brief
-	*   Diagnostics - print some basic information about the driver.
-	*/
+	 * @brief Diagnostics - print some basic information about the driver.
+	 */
 	void print_info();
 
 	/**
-	 * @brief
-	 *   print registers to console
+	 * @brief print registers to console
 	 */
 	virtual void print_registers() {};
 
 protected:
 
-	uint32_t	get_measure_interval() const { return _measure_interval; }
+	uint32_t get_measure_interval() const { return _measure_interval; }
 
-	virtual int	measure() = 0;
-	virtual int	collect() = 0;
+	virtual int collect() = 0;
 
-	virtual int	reset_sensor() { return PX4_ERROR; };
+	virtual int measure() = 0;
+
+	virtual int reset_sensor() { return PX4_ERROR; };
 
 	PX4Rangefinder	_px4_rangefinder;
 
-	perf_counter_t	_sample_perf;
-	perf_counter_t	_sample_interval_perf;
-	perf_counter_t	_comms_errors;
-	perf_counter_t	_sensor_resets;
-	perf_counter_t	_sensor_zero_resets;
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "ll40ls: comms errors")};
+	perf_counter_t _sample_interval_perf{perf_alloc(PC_ELAPSED, "ll40ls: interval")};
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "ll40ls: read")};
+	perf_counter_t _sensor_resets{perf_alloc(PC_COUNT, "ll40ls: resets")};
+	perf_counter_t _sensor_zero_resets{perf_alloc(PC_COUNT, "ll40ls: zero resets")};
 
 private:
 
-	uint32_t            _measure_interval{LL40LS_CONVERSION_INTERVAL};
+	uint32_t  _measure_interval{LL40LS_CONVERSION_INTERVAL};
 };
diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
index 52389ba354..90702e0d44 100644
--- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
@@ -137,7 +137,8 @@ LidarLiteI2C::probe()
 			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;
-				goto ok;
+				_retries = 3;
+				return reset_sensor();
 			}
 
 			PX4_DEBUG("probe failed unit_id=0x%02x", (unsigned)_unit_id);
@@ -149,22 +150,17 @@ LidarLiteI2C::probe()
 			 */
 			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);
-				goto ok;
+				_retries = 3;
+				return reset_sensor();
 			}
 
 			PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x", (unsigned)_hw_version, (unsigned)_sw_version);
 		}
-
 	}
 
 	// not found on any address
 	return -EIO;
-
-ok:
-	_retries = 3;
-	return reset_sensor();
 }
 
 int
@@ -180,8 +176,7 @@ LidarLiteI2C::measure()
 	/*
 	 * Send the command to begin a measurement.
 	 */
-	const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
-	int ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0);
+	int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE);
 
 	if (OK != ret) {
 		perf_count(_comms_errors);