refactor ll40ls: use driver base class

ROMFS/px4fmu_common/init.d/rc.sensors

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

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

@@ -16,7 +16,7 @@ hmc5883 -X start
 
 ist8310 -I -R 4 start
 
-ll40ls start i2c -a
+ll40ls start -X
 
 # Internal SPI (auto detect ms5611 or ms5607)
 if ! ms5611 -T 5607 -s start

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -41,9 +41,10 @@
 
 #include "LidarLiteI2C.h"
 
-LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int address) :
+LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
-	I2C("LL40LS", nullptr, bus, address, 100000),
+			   const int address) :
-	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
+	I2C("LL40LS", nullptr, bus, address, bus_frequency),
+	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
 	_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation)
 {
 	_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
@@ -55,7 +56,6 @@ LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int addr
 
 LidarLiteI2C::~LidarLiteI2C()
 {
-	stop();
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
 	perf_free(_sensor_resets);
@@ -74,8 +74,9 @@ LidarLiteI2C::init()
 }
 
 void
-LidarLiteI2C::print_info()
+LidarLiteI2C::print_status()
 {
+	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_sensor_resets);
@@ -457,12 +458,7 @@ void LidarLiteI2C::start()
 	ScheduleNow();
 }
 
-void LidarLiteI2C::stop()
+void LidarLiteI2C::RunImpl()
-{
-	ScheduleClear();
-}
-
-void LidarLiteI2C::Run()
 {
 	/* collection phase? */
 	if (_collect_phase) {

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -45,6 +45,7 @@
 #include <mathlib/mathlib.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/i2c_spi_buses.h>
 #include <drivers/device/device.h>
 #include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
 #include <perf/perf_counter.h>
@@ -90,13 +91,18 @@ static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{50_ms};
 static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms};
 
 
-class LidarLiteI2C : public device::I2C, public px4::ScheduledWorkItem
+class LidarLiteI2C : public device::I2C, public I2CSPIDriver<LidarLiteI2C>
 {
 public:
-	LidarLiteI2C(const int bus, const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
+	LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
 		     const int address = LL40LS_BASEADDR);
 	virtual ~LidarLiteI2C();
 
+	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+					     int runtime_instance);
+	static void print_usage();
+
+
 	int init();
 
 	/**
@@ -112,14 +118,17 @@ public:
 	 */
 	void start();
 
-	void print_info();
-
 	/**
-	 * Stop the automatic measurement state machine.
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
 	 */
-	void stop();
+	void RunImpl();
 
 protected:
+	void custom_method(const BusCLIArguments &cli) override;
+
+	void print_status() override;
+
 	uint32_t get_measure_interval() const { return LL40LS_CONVERSION_INTERVAL; };
 
 	int measure();
@@ -161,12 +170,6 @@ private:
 	 */
 	int probe_address(const uint8_t address);
 
-	/**
-	 * Perform a poll cycle; collect from the previous measurement
-	 * and start a new one.
-	 */
-	void Run() override;
-
 	bool _collect_phase{false};
 	bool _is_v3hp{false};
 	bool _pause_measurements{false};

src/drivers/distance_sensor/ll40ls/ll40ls.cpp

@@ -38,151 +38,16 @@
  * @author Ban Siesta <bansiesta@gmail.com>
  * @author James Goppert <james.goppert@gmail.com>
  *
- * Interface for the PulsedLight Lidar-Lite range finders.
  */
 
-#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_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
 #include "LidarLiteI2C.h"
 
-
+void
-/**
+LidarLiteI2C::print_usage()
- * @brief Local functions in support of the shell command.
- */
-namespace ll40ls
-{
-
-LidarLiteI2C *instance = nullptr;
-
-int print_regs();
-int start(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
-int start_bus(const int bus = PX4_I2C_BUS_EXPANSION,
-	      const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
-int status();
-int stop();
-int usage();
-
-/**
- * @brief Prints register information to the console.
- */
-int
-print_regs()
-{
-	if (!instance) {
-		PX4_ERR("No ll40ls driver running");
-		return PX4_ERROR;
-	}
-
-	instance->print_registers();
-	return PX4_OK;
-}
-
-/**
- * Attempt to start driver on all available I2C busses.
- *
- * This function will return as soon as the first sensor
- * is detected on one of the available busses or if no
- * sensors are detected.
- */
-int
-start(const uint8_t rotation)
-{
-	if (instance != nullptr) {
-		PX4_ERR("already started");
-		return PX4_ERROR;
-	}
-
-	for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
-		if (start_bus(i2c_bus_options[i], rotation) == PX4_OK) {
-			return PX4_OK;
-		}
-	}
-
-	return PX4_ERROR;
-}
-
-/**
- * Start the driver on a specific bus.
- *
- * This function only returns if the sensor is up and running
- * or could not be detected successfully.
- */
-int
-start_bus(const int bus, const uint8_t rotation)
-{
-	if (instance != nullptr) {
-		PX4_ERR("already started");
-		return PX4_OK;
-	}
-
-	// Instantiate the driver.
-	instance = new LidarLiteI2C(bus, 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 on bus = %u", bus);
-		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
- */
-int
-stop()
-{
-	if (instance != nullptr) {
-		delete instance;
-		instance = nullptr;
-	}
-
-	PX4_INFO("driver stopped");
-	return PX4_OK;
-}
-
-/**
- * @brief Displays driver usage at the console.
- */
-int
-usage()
 {
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
@@ -193,97 +58,82 @@ I2C bus driver for LidarLite rangefinders.
 The sensor/driver must be enabled using the parameter SENS_EN_LL40LS.
 
 Setup/usage information: https://docs.px4.io/en/sensor/lidar_lite.html
-
-### 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("start");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAM_FLAG('a', "Attempt to start driver on all I2C buses (first one found)", true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true);
+	PRINT_MODULE_USAGE_COMMAND("regdump");
-	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-	PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status information");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
-	return PX4_OK;
 }
 
-} // namespace ll40ls
+I2CSPIDriverBase *LidarLiteI2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				      int runtime_instance)
+{
+	LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.rotation, cli.bus_frequency);
 
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (instance->init() != PX4_OK) {
+		delete instance;
+		return nullptr;
+	}
+
+	instance->start();
+	return instance;
+}
+
+void
+LidarLiteI2C::custom_method(const BusCLIArguments &cli)
+{
+	print_registers();
+}
 
-/**
- * @brief Driver 'main' command.
- */
 extern "C" __EXPORT int ll40ls_main(int argc, char *argv[])
 {
-	const char *myoptarg = nullptr;
+	int ch;
+	using ThisDriver = LidarLiteI2C;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 100000;
 
-	int bus = PX4_I2C_BUS_EXPANSION;
+	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
-	int ch = 0;
-	int myoptind = 1;
-
-	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
-
-	bool start_i2c_all = false;
-
-	while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		case 'a':
-			start_i2c_all = true;
-			break;
-
-		case 'b':
-			bus = atoi(myoptarg);
-			break;
-
 		case 'R':
-			rotation = (uint8_t)atoi(myoptarg);
+			cli.rotation = (enum Rotation)atoi(cli.optarg());
-			PX4_INFO("Setting Lidar orientation to %d", (int)rotation);
 			break;
-
-		default:
-			return ll40ls::usage();
 		}
 	}
 
-	if (myoptind >= argc) {
+	const char *verb = cli.optarg();
-		return ll40ls::usage();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
 	}
 
-	// Print the sensor register values.
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_LL40LS);
-	if (!strcmp(argv[myoptind], "print_regs")) {
+
-		return ll40ls::print_regs();
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
 	}
 
-	// Start the driver.
+	if (!strcmp(verb, "stop")) {
-	if (!strcmp(argv[myoptind], "start")) {
+		return ThisDriver::module_stop(iterator);
-		if (start_i2c_all) {
-			PX4_INFO("starting all i2c busses");
-			return ll40ls::start(rotation);
-
-		} else {
-			return ll40ls::start_bus(bus, rotation);
-		}
 	}
 
-	// Print the driver status.
+	if (!strcmp(verb, "status")) {
-	if (!strcmp(argv[myoptind], "status")) {
+		return ThisDriver::module_status(iterator);
-		return ll40ls::status();
 	}
 
-	// Stop the driver
+	if (!strcmp(verb, "regdump")) {
-	if (!strcmp(argv[myoptind], "stop")) {
+		return ThisDriver::module_custom_method(cli, iterator);
-		return ll40ls::stop();
 	}
 
-	// Print driver usage information.
+	ThisDriver::print_usage();
-	return ll40ls::usage();
+	return -1;
 }

src/drivers/drv_sensor.h

@@ -129,6 +129,7 @@
 #define DRV_OSD_DEVTYPE_ATXXXX		0x6b
 #define DRV_FLOW_DEVTYPE_PMW3901	0x6c
 
+#define DRV_DIST_DEVTYPE_LL40LS     0x70
 
 #define DRV_DEVTYPE_UNUSED		0xff