Refactor the ll40ls namespace driver methods to more closely match the cm8jl65, mappydot, leddar_one, and other distance sensor driver implementations.

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -64,7 +64,7 @@ fi
 # Lidar-Lite on I2C
 if param compare SENS_EN_LL40LS 2
 then
-	ll40ls start i2c
+	ll40ls start i2c -a
 fi
 
 # mappydot lidar sensor

boards/intel/aerofc-v1/init/rc.board_sensors

@@ -13,7 +13,7 @@ hmc5883 -X start
 ist8310 -C -b 1 -R 4 start
 aerofc_adc start
 
-ll40ls start i2c
+ll40ls start i2c -a
 
 # Internal SPI (auto detect ms5611 or ms5607)
 ms5611 -T 0 -s start

src/drivers/distance_sensor/ll40ls/LidarLite.cpp

@@ -41,7 +41,7 @@
 
 #include "LidarLite.h"
 
-LidarLite::LidarLite(uint8_t rotation) :
+LidarLite::LidarLite(const uint8_t rotation) :
 	_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation)
 {
 	_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);

src/drivers/distance_sensor/ll40ls/LidarLite.h

@@ -44,22 +44,24 @@
 #include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
 #include <perf/perf_counter.h>
 
+using namespace time_literals;
+
 /* Device limits */
-#define LL40LS_MIN_DISTANCE (0.05f)
+#define LL40LS_MIN_DISTANCE    (0.05f)
-#define LL40LS_MAX_DISTANCE (25.00f)
+#define LL40LS_MAX_DISTANCE    (25.00f)
 #define LL40LS_MAX_DISTANCE_V2 (35.00f)
 
 
-// normal conversion wait time
+// Normal conversion wait time.
-#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */
+#define LL40LS_CONVERSION_INTERVAL 50_ms
 
-// maximum time to wait for a conversion to complete.
+// Maximum time to wait for a conversion to complete.
-#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */
+#define LL40LS_CONVERSION_TIMEOUT 100_ms
 
 class LidarLite
 {
 public:
-	LidarLite(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	LidarLite(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	virtual ~LidarLite();
 
 	virtual int init() = 0;
@@ -72,13 +74,13 @@ public:
 	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 collect() = 0;
 

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -41,11 +41,7 @@
 
 #include "LidarLiteI2C.h"
 
-#include <px4_defines.h>
+LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int address) :
-#include <mathlib/mathlib.h>
-#include <drivers/drv_hrt.h>
-
-LidarLiteI2C::LidarLiteI2C(int bus, uint8_t rotation, int address) :
 	LidarLite(rotation),
 	I2C("LL40LS", nullptr, bus, address, 100000),
 	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
@@ -62,45 +58,34 @@ LidarLiteI2C::~LidarLiteI2C()
 int
 LidarLiteI2C::init()
 {
-	int ret = PX4_ERROR;
+	// Perform I2C init (and probe) first.
-
+	if (I2C::init() != PX4_OK) {
-	/* do I2C init (and probe) first */
+		return PX4_ERROR;
-	if (I2C::init() != OK) {
-		return ret;
 	}
 
-	/* sensor is ok, but we don't really know if it is within range */
+	return PX4_OK;
-	_sensor_ok = true;
-
-	start();
-
-	return OK;
 }
 
 int
-LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val)
+LidarLiteI2C::read_reg(const uint8_t reg, uint8_t &val)
 {
 	return lidar_transfer(&reg, 1, &val, 1);
 }
 
 int
-LidarLiteI2C::write_reg(uint8_t reg, uint8_t val)
+LidarLiteI2C::write_reg(const uint8_t reg, const uint8_t &val)
 {
 	const uint8_t cmd[2] = { reg, val };
 	return transfer(&cmd[0], 2, nullptr, 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)
+LidarLiteI2C::lidar_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
 {
 	if (send != nullptr && send_len > 0) {
 		int ret = transfer(send, send_len, nullptr, 0);
 
-		if (ret != OK) {
+		if (ret != PX4_OK) {
 			return ret;
 		}
 	}
@@ -109,7 +94,7 @@ LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *re
 		return transfer(nullptr, 0, recv, recv_len);
 	}
 
-	return OK;
+	return PX4_ERROR;
 }
 
 int
@@ -135,11 +120,6 @@ 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.
 		 */
-
-
-		/**
-		 * 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)) {
 
@@ -164,7 +144,7 @@ LidarLiteI2C::probe()
 
 				if (_unit_id > 0) {
 					// v3hp
-					_is_V3hp = true;
+					_is_v3hp = true;
 					PX4_INFO("probe success - id: %u", _unit_id);
 				}
 			}
@@ -194,12 +174,10 @@ LidarLiteI2C::measure()
 		return OK;
 	}
 
-	/*
+	// Send the command to begin a measurement.
-	 * Send the command to begin a measurement.
-	 */
 	int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE);
 
-	if (OK != ret) {
+	if (ret != PX4_OK) {
 		perf_count(_comms_errors);
 		PX4_DEBUG("i2c::transfer returned %d", ret);
 
@@ -220,9 +198,6 @@ LidarLiteI2C::measure()
 	return OK;
 }
 
-/*
-  reset the sensor to power on defaults plus additional configurations
- */
 int
 LidarLiteI2C::reset_sensor()
 {
@@ -267,10 +242,8 @@ LidarLiteI2C::reset_sensor()
 	return OK;
 }
 
-/*
+void
-  dump sensor registers for debugging
+LidarLiteI2C::print_registers()
- */
-void LidarLiteI2C::print_registers()
 {
 	_pause_measurements = true;
 	PX4_INFO("registers");
@@ -297,9 +270,10 @@ void LidarLiteI2C::print_registers()
 	_pause_measurements = false;
 }
 
-int LidarLiteI2C::collect()
+int
+LidarLiteI2C::collect()
 {
-	/* read from the sensor */
+	// read from the sensor
 	uint8_t val[2] {};
 
 	perf_begin(_sample_perf);
@@ -359,8 +333,8 @@ int 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
+	// Relative signal strength measurement, i.e. the strength of
-	 * the main signal peak compared to the general noise level*/
+	// 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);
 
@@ -396,13 +370,13 @@ int LidarLiteI2C::collect()
 	uint8_t signal_value = 0;
 
 	// We detect if V3HP is being used
-	if (_is_V3hp) {
+	if (_is_v3hp) {
 		signal_min = LL40LS_SIGNAL_STRENGTH_MIN_V3HP;
 		signal_max = LL40LS_SIGNAL_STRENGTH_MAX_V3HP;
 		signal_value = ll40ls_signal_strength;
 
 	} else {
-		/* Absolute peak strength measurement, i.e. absolute strength of main signal peak*/
+		// 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);
 
@@ -442,10 +416,9 @@ int LidarLiteI2C::collect()
 		} else {
 			signal_value = ll40ls_peak_strength;
 		}
-
 	}
 
-	/* Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments*/
+	// 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(signal_value - signal_min, 0) / (signal_max - signal_min);
 
@@ -462,10 +435,10 @@ int LidarLiteI2C::collect()
 
 void LidarLiteI2C::start()
 {
-	/* reset the report ring and state machine */
+	// reset the report ring and state machine
 	_collect_phase = false;
 
-	/* schedule a cycle to start things */
+	// schedule a cycle to start things
 	ScheduleNow();
 }
 

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -42,114 +42,127 @@
 
 #include "LidarLite.h"
 
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+#include <mathlib/mathlib.h>
+#include <px4_defines.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
-#include <drivers/device/i2c.h>
 
 /* Configuration Constants */
-static constexpr uint8_t LL40LS_BASEADDR	= 0x62;	/* 7-bit address */
+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_BASEADDR_OLD          = 0x42;   /* previous 7-bit address */
-static constexpr uint8_t LL40LS_SIG_COUNT_VAL_DEFAULT = 0x80; /* Default maximum acquisition count */
+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 */
+static constexpr uint8_t LL40LS_MEASURE_REG         = 0x00;     /* Measure range register */
-static constexpr uint8_t LL40LS_MSRREG_RESET	= 0x00;	/* reset to power on defaults */
+static constexpr uint8_t LL40LS_MSRREG_RESET        = 0x00;     /* reset to power on defaults */
-static constexpr uint8_t LL40LS_MSRREG_ACQUIRE	=
+static constexpr uint8_t LL40LS_MSRREG_ACQUIRE      =
-	0x04;	/* Value to initiate a measurement, varies based on sensor revision */
+	0x04;     /* Value to initiate a measurement, varies based on sensor revision */
-static constexpr uint8_t LL40LS_DISTHIGH_REG	= 0x0F;	/* High byte of distance register, auto increment */
+static constexpr uint8_t LL40LS_DISTHIGH_REG        = 0x0F;     /* High byte of distance register, auto increment */
-static constexpr uint8_t LL40LS_AUTO_INCREMENT	= 0x80;
+static constexpr uint8_t LL40LS_AUTO_INCREMENT      = 0x80;
-static constexpr uint8_t LL40LS_HW_VERSION	= 0x41;
+static constexpr uint8_t LL40LS_HW_VERSION          = 0x41;
-static constexpr uint8_t LL40LS_SW_VERSION	= 0x4f;
+static constexpr uint8_t LL40LS_SW_VERSION          = 0x4f;
-static constexpr uint8_t LL40LS_SIGNAL_STRENGTH_REG	= 0x0e;
+static constexpr uint8_t LL40LS_SIGNAL_STRENGTH_REG = 0x0e;
-static constexpr uint8_t LL40LS_PEAK_STRENGTH_REG	= 0x0c;
+static constexpr uint8_t LL40LS_PEAK_STRENGTH_REG   = 0x0c;
-static constexpr uint8_t LL40LS_UNIT_ID_HIGH	= 0x16;
+static constexpr uint8_t LL40LS_UNIT_ID_HIGH        = 0x16;
-static constexpr uint8_t LL40LS_UNIT_ID_LOW	= 0x17;
+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_REG   = 0x02;     /* Maximum acquisition count register */
-static constexpr uint8_t LL40LS_SIG_COUNT_VAL_MAX	= 0xFF;	/* Maximum acquisition count max value */
+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 =
+static constexpr int LL40LS_SIGNAL_STRENGTH_MIN_V3HP = 70;      /* Min signal strength for V3HP */
-	24;	// Minimum (relative) signal strength value for accepting a measurement
+static constexpr int LL40LS_SIGNAL_STRENGTH_MAX_V3HP = 255;     /* Max signal strength for V3HP */
-static constexpr int LL40LS_PEAK_STRENGTH_LOW	= 135;	// Minimum peak strength raw value for accepting a measurement
+static constexpr int LL40LS_SIGNAL_STRENGTH_LOW      =
-static constexpr int LL40LS_PEAK_STRENGTH_HIGH	= 234;	// Max peak strength raw value
+	24;      /* Minimum (relative) signal strength value for accepting a measurement */
+static constexpr int LL40LS_PEAK_STRENGTH_LOW        =
+	135;     /* Minimum peak strength raw value for accepting a measurement */
+static constexpr int LL40LS_PEAK_STRENGTH_HIGH       = 234;     /* Max peak strength raw value */
 
 
 class LidarLiteI2C : public LidarLite, public device::I2C, public px4::ScheduledWorkItem
 {
 public:
-	LidarLiteI2C(int bus, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int address = LL40LS_BASEADDR);
+	LidarLiteI2C(const int bus, const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
+		     const int address = LL40LS_BASEADDR);
 	virtual ~LidarLiteI2C();
 
-	int		init() override;
+	int init() override;
 
-	void		print_registers() override;
+	/**
+	 * Print sensor registers to console for debugging.
+	 */
+	void print_registers() override;
+
+	/**
+	 * Initialise the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
+	void start() override;
+
+	/**
+	 * Stop the automatic measurement state machine.
+	 */
+	void stop() override;
 
 protected:
-	int		probe() override;
-	int		read_reg(uint8_t reg, uint8_t &val);
-	int		write_reg(uint8_t reg, uint8_t val);
 
-	int		measure() override;
+	int measure() override;
-	int		reset_sensor() override;
+
+	/**
+	 * Reset the sensor to power on defaults plus additional configurations.
+	 */
+	int reset_sensor() override;
+
+	int probe() override;
+
+	int read_reg(const uint8_t reg, uint8_t &val);
+
+	int write_reg(const uint8_t reg, const uint8_t &val);
 
 private:
 
+	int collect() override;
+
 	/**
 	 * LidarLite specific transfer function. This is needed
 	 * to avoid a stop transition with SCL high
 	 *
-	 * @param send		Pointer to bytes to send.
+	 * @param send          Pointer to bytes to send.
-	 * @param send_len	Number of bytes to send.
+	 * @param send_len      Number of bytes to send.
-	 * @param recv		Pointer to buffer for bytes received.
+	 * @param recv          Pointer to buffer for bytes received.
-	 * @param recv_len	Number of bytes to receive.
+	 * @param recv_len      Number of bytes to receive.
-	 * @return		OK if the transfer was successful, -errno
+	 * @return              OK if the transfer was successful, -errno
-	 *			otherwise.
+	 *                      otherwise.
 	 */
-	int                 lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+	int lidar_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
 
 	/**
-	* Test whether the device supported by the driver is present at a
+	 * Test whether the device supported by the driver is present at a
-	* specific address.
+	 * specific address.
-	*
+	 *
-	* @param address    The I2C bus address to probe.
+	 * @param address The I2C bus address to probe.
-	* @return       True if the device is present.
+	 * @return        True if the device is present.
-	*/
+	 */
-	int                 probe_address(uint8_t address);
+	int probe_address(const uint8_t address);
 
 	/**
-	* Initialise the automatic measurement state machine and start it.
+	 * Perform a poll cycle; collect from the previous measurement
-	*
+	 * and start a new one.
-	* @note This function is called at open and error time.  It might make sense
+	 */
-	*       to make it more aggressive about resetting the bus in case of errors.
+	void Run() override;
-	*/
-	void                start() override;
 
-	/**
+	bool _collect_phase{false};
-	* Stop the automatic measurement state machine.
+	bool _is_v3hp{false};
-	*/
+	bool _pause_measurements{false};
-	void                stop() override;
 
-	/**
+	uint8_t _hw_version{0};
-	* Perform a poll cycle; collect from the previous measurement
+	uint8_t _sw_version{0};
-	* and start a new one.
-	*/
-	void                Run() override;
-	int                 collect() override;
 
+	uint16_t _unit_id{0};
+	uint16_t _zero_counter{0};
 
-	bool		_sensor_ok{false};
+	uint64_t _acquire_time_usec{0};
-	bool		_collect_phase{false};
-
-	uint16_t	_zero_counter{0};
-	uint64_t	_acquire_time_usec{0};
-
-	bool		_pause_measurements{false};
-
-	uint8_t		_hw_version{0};
-	uint8_t		_sw_version{0};
-	uint16_t	_unit_id{0};
-	bool 		_is_V3hp{false};
-
 };

src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp

@@ -43,13 +43,8 @@
  */
 
 #include "LidarLitePWM.h"
-#include <stdio.h>
-#include <string.h>
-#include <px4_defines.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_pwm_input.h>
 
-LidarLitePWM::LidarLitePWM(uint8_t rotation) :
+LidarLitePWM::LidarLitePWM(const uint8_t rotation) :
 	LidarLite(rotation),
 	ScheduledWorkItem(px4::wq_configurations::hp_default)
 {

src/drivers/distance_sensor/ll40ls/LidarLitePWM.h

@@ -45,13 +45,19 @@
 
 #include "LidarLite.h"
 
+#include <stdio.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_input.h>
+#include <px4_defines.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/topics/pwm_input.h>
 
 class LidarLitePWM : public LidarLite, public px4::ScheduledWorkItem
 {
 public:
-	LidarLitePWM(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	LidarLitePWM(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	virtual ~LidarLitePWM();
 
 	int init() override;
@@ -62,12 +68,12 @@ public:
 
 protected:
 
-	int measure() override;
 	int collect() override;
+	int measure() override;
 
 private:
 
 	int _pwmSub{-1};
-	pwm_input_s _pwm{};
 
+	pwm_input_s _pwm{};
 };

src/drivers/distance_sensor/ll40ls/ll40ls.cpp

@@ -41,45 +41,19 @@
  * Interface for the PulsedLight Lidar-Lite range finders.
  */
 
-#include "LidarLiteI2C.h"
-#include "LidarLitePWM.h"
-#include <board_config.h>
-#include <systemlib/err.h>
-#include <fcntl.h>
 #include <cstdlib>
+#include <fcntl.h>
 #include <string.h>
 #include <stdio.h>
+#include <systemlib/err.h>
+
+#include <board_config.h>
+#include <drivers/device/i2c.h>
 #include <px4_getopt.h>
+#include <px4_module.h>
 
-enum LL40LS_BUS {
+#include "LidarLiteI2C.h"
-	LL40LS_BUS_I2C_ALL = 0,
+#include "LidarLitePWM.h"
-	LL40LS_BUS_I2C_INTERNAL,
-	LL40LS_BUS_I2C_EXTERNAL,
-	LL40LS_BUS_PWM
-};
-
-static constexpr struct ll40ls_bus_option {
-	enum LL40LS_BUS busid;
-	uint8_t busnum;
-} bus_options[] = {
-#ifdef PX4_I2C_BUS_EXPANSION
-	{ LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION },
-#endif
-#ifdef PX4_I2C_BUS_EXPANSION1
-	{ LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION1 },
-#endif
-#ifdef PX4_I2C_BUS_EXPANSION2
-	{ LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION2 },
-#endif
-#ifdef PX4_I2C_BUS_ONBOARD
-	{ LL40LS_BUS_I2C_INTERNAL, PX4_I2C_BUS_ONBOARD },
-#endif
-};
-
-/**
- * @brief Driver 'main' command.
- */
-extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
 
 
 /**
@@ -89,159 +63,227 @@ 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);
+int print_regs();
-void    stop();
+int start(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
-void    info();
+int start_bus(const int bus = PX4_I2C_BUS_EXPANSION,
-void    regdump();
+	      const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
-void    usage();
+int start_pwm(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+int status();
+int stop();
+int usage();
 
 /**
- * @brief Starts the driver.
+ * @brief Prints register information to the console.
  */
-void start(enum LL40LS_BUS busid, uint8_t rotation)
+int
+print_regs()
 {
-	if (instance) {
+	if (!instance) {
-		PX4_INFO("driver already started");
+		PX4_ERR("No ll40ls driver running");
-		return;
+		return PX4_ERROR;
 	}
 
-	if (busid == LL40LS_BUS_PWM) {
+	instance->print_registers();
-		instance = new LidarLitePWM(rotation);
+	return PX4_OK;
+}
 
-		if (!instance) {
+/**
-			PX4_ERR("Failed to instantiate LidarLitePWM");
+ * Attempt to start driver on all available I2C busses.
-			return;
+ *
-		}
+ * This function will return as soon as the first sensor
-
+ * is detected on one of the available busses or if no
-		if (instance->init() != PX4_OK) {
+ * sensors are detected.
-			PX4_ERR("failed to initialize LidarLitePWM");
+ */
-			stop();
+int
-			return;
+start(const uint8_t rotation)
-		}
+{
-
+	if (instance != nullptr) {
-	} else {
+		PX4_ERR("already started");
-		for (uint8_t i = 0; i < (sizeof(bus_options) / sizeof(bus_options[0])); i++) {
+		return PX4_ERROR;
-			if (busid != LL40LS_BUS_I2C_ALL && busid != bus_options[i].busid) {
+	}
-				continue;
-			}
-
-			if (start_bus(bus_options[i], rotation) == PX4_OK) {
-				return;
-			}
 
+	for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
+		if (start_bus(i2c_bus_options[i], rotation) == PX4_OK) {
+			return PX4_OK;
 		}
 	}
 
-	if (instance == nullptr) {
+	return PX4_ERROR;
-		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
+ * This function only returns if the sensor is up and running
- * or failed to detect the sensor.
+ * or could not be detected successfully.
  */
 int
-start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation)
+start_bus(const int bus, const uint8_t rotation)
 {
-	instance = new LidarLiteI2C(i2c_bus.busnum, rotation);
+	if (instance != nullptr) {
+		PX4_ERR("already started");
+		return PX4_OK;
+	}
 
-	if (instance->init() != OK) {
+	// Instantiate the driver.
-		stop();
+	instance = new LidarLiteI2C(bus, rotation);
-		PX4_INFO("LidarLiteI2C - no device on bus %u", (unsigned)i2c_bus.busid);
+
+	if (instance == nullptr) {
+		PX4_ERR("Failed to instantiate the driver");
+		delete instance;
 		return PX4_ERROR;
 	}
 
+	// Initialize the sensor.
+	if (instance->init() != PX4_OK) {
+		PX4_ERR("Failed to initialize LidarLite on bus = %u", bus);
+		delete instance;
+		instance = nullptr;
+		return PX4_ERROR;
+	}
+
+	// Start the driver.
+	instance->start();
+
+	PX4_INFO("driver started");
+	return PX4_OK;
+}
+
+/**
+ * Start the pwm driver.
+ *
+ * This function only returns if the sensor is up and running
+ * or could not be detected successfully.
+ */
+int
+start_pwm(const uint8_t rotation)
+{
+	if (instance != nullptr) {
+		PX4_ERR("already started");
+		return PX4_OK;
+	}
+
+	instance = new LidarLitePWM(rotation);
+
+	if (instance == nullptr) {
+		PX4_ERR("Failed to instantiate the driver");
+		delete instance;
+		return PX4_ERROR;
+	}
+
+	// Initialize the sensor.
+	if (instance->init() != PX4_OK) {
+		PX4_ERR("Failed to initialize LidarLite pwm.");
+		delete instance;
+		instance = nullptr;
+		return PX4_ERROR;
+	}
+
+	// Start the driver.
+	instance->start();
+
+	PX4_INFO("driver started");
+	return PX4_OK;
+}
+
+/**
+ * @brief Prints status info about the driver.
+ */
+int
+status()
+{
+	if (instance == nullptr) {
+		PX4_ERR("driver not running");
+		return PX4_ERROR;
+	}
+
+	instance->print_info();
 	return PX4_OK;
 }
 
 /**
  * @brief Stops the driver
  */
-void stop()
+int
+stop()
 {
 	if (instance != nullptr) {
 		delete instance;
 		instance = nullptr;
-
-	} else {
-		PX4_ERR("driver not running");
 	}
 
-
+	PX4_INFO("driver stopped");
-}
+	return PX4_OK;
-
-/**
- * @brief Prints status info about the driver.
- */
-void
-info()
-{
-	if (!instance) {
-		warnx("No ll40ls driver running");
-		return;
-	}
-
-	printf("state @ %p\n", instance);
-	instance->print_info();
-}
-
-/**
- * @brief Dumps the register information.
- */
-void
-regdump()
-{
-	if (!instance) {
-		warnx("No ll40ls driver running");
-		return;
-	}
-
-	printf("regdump @ %p\n", instance);
-	instance->print_registers();
 }
 
 /**
  * @brief Displays driver usage at the console.
  */
-void
+int
 usage()
 {
-	PX4_INFO("missing command: try 'start', 'stop', 'info', 'info' or 'regdump' [i2c|pwm]");
+	PRINT_MODULE_DESCRIPTION(
-	PX4_INFO("options for I2C:");
+		R"DESCR_STR(
-	PX4_INFO("    -X only external bus");
+### Description
-#ifdef PX4_I2C_BUS_ONBOARD
+
-	PX4_INFO("    -I only internal bus");
+I2C bus driver for LidarLite rangefinders.
-#endif
+
-	PX4_INFO("E.g. ll40ls start i2c -R 0");
+The sensor/driver must be enabled using the parameter SENS_EN_LL40LS.
+
+Setup/usage information: https://docs.px4.io/v1.9.0/en/sensor/lidar_lite.htmls
+
+### Examples
+
+Start driver on any bus (start on bus where first sensor found).
+$ ll40ls start i2c -a
+Start driver on specified bus
+$ ll40ls start i2c -b 1
+Stop driver
+$ ll40ls stop
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("ll40ls", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("print_regs","Print the register values");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "PWM device");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "I2C device");
+	PRINT_MODULE_USAGE_PARAM_FLAG('a', "Attempt to start driver on all I2C buses (first one found)", true);
+	PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status information");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
+	return PX4_OK;
 }
 
 } // namespace ll40ls
 
-int
+
-ll40ls_main(int argc, char *argv[])
+/**
+ * @brief Driver 'main' command.
+ */
+extern "C" __EXPORT int ll40ls_main(int argc, char *argv[])
 {
-	int ch;
-	int myoptind = 1;
 	const char *myoptarg = nullptr;
-	enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL;
+
+	int bus = PX4_I2C_BUS_EXPANSION;
+	int ch = 0;
+	int myoptind = 1;
+
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 
-	while ((ch = px4_getopt(argc, argv, "IXR:", &myoptind, &myoptarg)) != EOF) {
+	bool start_i2c_all = false;
+	bool start_pwm = false;
+
+	while ((ch = px4_getopt(argc, argv, "ab:R", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-#ifdef PX4_I2C_BUS_ONBOARD
+		case 'a':
-
+			start_i2c_all = true;
-		case 'I':
-			busid = LL40LS_BUS_I2C_INTERNAL;
 			break;
-#endif
 
-		case 'X':
+		case 'b':
-			busid = LL40LS_BUS_I2C_EXTERNAL;
+			bus = atoi(myoptarg);
 			break;
 
 		case 'R':
@@ -250,52 +292,61 @@ ll40ls_main(int argc, char *argv[])
 			break;
 
 		default:
-			ll40ls::usage();
+			return ll40ls::usage();
-			return 0;
 		}
 	}
 
-	/* Determine protocol first because it's needed next. */
+	// Determine protocol.
 	if (argc > myoptind + 1) {
 		const char *protocol = argv[myoptind + 1];
 
-		if (!strcmp(protocol, "pwm")) {
+		if (!strcmp(protocol, "i2c")) {
-			busid = LL40LS_BUS_PWM;;
+			PX4_INFO("protocol %s", protocol);
 
-		} else if (!strcmp(protocol, "i2c")) {
+		} else if (!strcmp(protocol, "pwm")) {
-			// Do nothing
+			PX4_INFO("protocol %s", protocol);
+			start_pwm = true;
 
 		} else {
-			warnx("unknown protocol, choose pwm or i2c");
+			PX4_INFO("unknown protocol, choose pwm or i2c");
-			ll40ls::usage();
+			return ll40ls::usage();
-			return 0;
 		}
 	}
 
-	/* Now determine action. */
+	if (myoptind >= argc) {
-	if (argc > myoptind) {
+		return ll40ls::usage();
-		const char *verb = argv[myoptind];
-
-		if (!strcmp(verb, "start")) {
-			ll40ls::start(busid, rotation);
-
-		} else if (!strcmp(verb, "stop")) {
-			ll40ls::stop();
-
-		} else if (!strcmp(verb, "regdump")) {
-			ll40ls::regdump();
-
-		} else if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
-			ll40ls::info();
-
-		} else {
-			ll40ls::usage();
-		}
-
-		return 0;
 	}
 
-	warnx("unrecognized command, try 'start', 'info' or 'regdump'");
+	// Print the sensor register values.
-	ll40ls::usage();
+	if (!strcmp(argv[myoptind], "print_regs")) {
-	return 0;
+		return ll40ls::print_regs();
+	}
+
+	// Start the driver.
+	if (!strcmp(argv[myoptind], "start")) {
+		if (start_i2c_all) {
+			PX4_INFO("starting all i2c busses");
+			return ll40ls::start(rotation);
+
+		} else if (start_pwm) {
+			PX4_INFO("starting pwm");
+			return ll40ls::start_pwm(rotation);
+
+		} else {
+			return ll40ls::start_bus(bus, rotation);
+		}
+	}
+
+	// Print the driver status.
+	if (!strcmp(argv[myoptind], "status")) {
+		return ll40ls::status();
+	}
+
+	// Stop the driver
+	if (!strcmp(argv[myoptind], "stop")) {
+		return ll40ls::stop();
+	}
+
+	// Print driver usage information.
+	return ll40ls::usage();
 }

src/lib/drivers/device/nuttx/I2C.cpp

@@ -51,7 +51,7 @@ namespace device
 
 unsigned int I2C::_bus_clocks[BOARD_NUMBER_I2C_BUSES] = BOARD_I2C_BUS_CLOCK_INIT;
 
-I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency) :
+I2C::I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency) :
 	CDev(name, devname),
 	_frequency(frequency)
 {
@@ -167,7 +167,7 @@ out:
 }
 
 int
-I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
 {
 	px4_i2c_msg_t msgv[2];
 	unsigned msgs;

src/lib/drivers/device/nuttx/I2C.hpp

@@ -81,7 +81,7 @@ protected:
 	 * @param address	I2C bus address, or zero if set_address will be used
 	 * @param frequency	I2C bus frequency for the device (currently not used)
 	 */
-	I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency);
+	I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency);
 	virtual ~I2C();
 
 	/**
@@ -101,7 +101,7 @@ protected:
 	 * @return		OK if the transfer was successful, -errno
 	 *			otherwise.
 	 */
-	int		transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+	int		transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
 
 	bool		external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }
 

src/lib/drivers/device/posix/I2C.cpp

@@ -59,7 +59,7 @@ static constexpr const int simulate = PX4_SIMULATE_I2C;
 namespace device
 {
 
-I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency) :
+I2C::I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency) :
 	CDev(name, devname)
 {
 	DEVICE_DEBUG("I2C::I2C name = %s devname = %s", name, devname);
@@ -119,7 +119,7 @@ I2C::init()
 }
 
 int
-I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
 {
 #ifndef __PX4_LINUX
 	return PX4_ERROR;

src/lib/drivers/device/posix/I2C.hpp

@@ -78,7 +78,7 @@ protected:
 	 * @param address	I2C bus address, or zero if set_address will be used
 	 * @param frequency	I2C bus frequency for the device (currently not used)
 	 */
-	I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency = 0);
+	I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency = 0);
 	virtual ~I2C();
 
 	/**
@@ -98,7 +98,7 @@ protected:
 	 * @return		OK if the transfer was successful, -errno
 	 *			otherwise.
 	 */
-	int		transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+	int		transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
 
 	bool		external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }