diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 387eda1af6..c81b5876c4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/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 diff --git a/boards/intel/aerofc-v1/init/rc.board_sensors b/boards/intel/aerofc-v1/init/rc.board_sensors index 10c55843a3..b2e4de1701 100644 --- a/boards/intel/aerofc-v1/init/rc.board_sensors +++ b/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 diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index 0e1f38095e..e8b45b0f2c 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/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) : - I2C("LL40LS", nullptr, bus, address, 100000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, + const int address) : + 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() -{ - ScheduleClear(); -} - -void LidarLiteI2C::Run() +void LidarLiteI2C::RunImpl() { /* collection phase? */ if (_collect_phase) { diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index 16c3acca46..2a3c592648 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -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 { 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}; diff --git a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp index 278dbc6eb1..ee5c52d78c 100644 --- a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp +++ b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp @@ -38,151 +38,16 @@ * @author Ban Siesta * @author James Goppert * - * Interface for the PulsedLight Lidar-Lite range finders. */ -#include -#include -#include -#include -#include - #include -#include #include #include #include "LidarLiteI2C.h" - -/** - * @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() +void +LidarLiteI2C::print_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_DESCR("start","Start driver"); - 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; + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_COMMAND("regdump"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -} // 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; - 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) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { - case 'a': - start_i2c_all = true; - break; - - case 'b': - bus = atoi(myoptarg); - break; - case 'R': - rotation = (uint8_t)atoi(myoptarg); - PX4_INFO("Setting Lidar orientation to %d", (int)rotation); + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; - - default: - return ll40ls::usage(); } } - if (myoptind >= argc) { - return ll40ls::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - // Print the sensor register values. - if (!strcmp(argv[myoptind], "print_regs")) { - return ll40ls::print_regs(); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_LL40LS); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - // Start the driver. - if (!strcmp(argv[myoptind], "start")) { - if (start_i2c_all) { - PX4_INFO("starting all i2c busses"); - return ll40ls::start(rotation); - - } else { - return ll40ls::start_bus(bus, rotation); - } + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - // Print the driver status. - if (!strcmp(argv[myoptind], "status")) { - return ll40ls::status(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - // Stop the driver - if (!strcmp(argv[myoptind], "stop")) { - return ll40ls::stop(); + if (!strcmp(verb, "regdump")) { + return ThisDriver::module_custom_method(cli, iterator); } - // Print driver usage information. - return ll40ls::usage(); + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 6a537ad956..f830fabc59 100644 --- a/src/drivers/drv_sensor.h +++ b/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