From a2d1f6ddced4d71c9ee533d24890f55e3431f1c9 Mon Sep 17 00:00:00 2001 From: Daniele Pettenuzzo <35664507+DanielePettenuzzo@users.noreply.github.com> Date: Thu, 4 Oct 2018 06:54:27 +0200 Subject: [PATCH] ll40ls driver: support also lidar lite v3HP (#10578) --- .../distance_sensor/ll40ls/LidarLite.cpp | 2 +- .../distance_sensor/ll40ls/LidarLite.h | 5 +- .../distance_sensor/ll40ls/LidarLiteI2C.cpp | 51 +++++++++++++++---- .../distance_sensor/ll40ls/LidarLiteI2C.h | 3 ++ src/drivers/distance_sensor/ll40ls/ll40ls.cpp | 2 +- 5 files changed, 48 insertions(+), 15 deletions(-) diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp index 4f7a602123..37156d836a 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp @@ -43,7 +43,7 @@ LidarLite::LidarLite() : _min_distance(LL40LS_MIN_DISTANCE), - _max_distance(LL40LS_MAX_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE_V3), _measure_ticks(0) { } diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.h b/src/drivers/distance_sensor/ll40ls/LidarLite.h index b59887bca0..1355bba527 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.h @@ -45,7 +45,8 @@ /* Device limits */ #define LL40LS_MIN_DISTANCE (0.05f) -#define LL40LS_MAX_DISTANCE (25.00f) +#define LL40LS_MAX_DISTANCE_V3 (35.00f) +#define LL40LS_MAX_DISTANCE_V1 (25.00f) // normal conversion wait time #define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */ @@ -86,7 +87,7 @@ protected: /** * Set the min and max distance thresholds if you want the end points of the sensors * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE - * and LL40LS_MAX_DISTANCE + * and LL40LS_MAX_DISTANCE_V3 */ void set_minimum_distance(const float min); void set_maximum_distance(const float max); diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index 995072bf6f..0e9e72fd44 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -69,7 +69,8 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr _acquire_time_usec(0), _pause_measurements(false), _hw_version(0), - _sw_version(0) + _sw_version(0), + _unit_id(0) { // up the retries since the device misses the first measure attempts _retries = 3; @@ -88,6 +89,10 @@ LidarLiteI2C::~LidarLiteI2C() delete _reports; } + if (_distance_sensor_topic != nullptr) { + orb_unadvertise(_distance_sensor_topic); + } + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); } @@ -172,22 +177,46 @@ int LidarLiteI2C::probe() // cope with both old and new I2C bus address const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + uint8_t id_high = 0, id_low = 0; + // more retries for detection _retries = 10; for (uint8_t i = 0; i < sizeof(addresses); i++) { - /* - 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; + + set_device_address(addresses[i]); + + if (addresses[i] == LL40LS_BASEADDR) { + + /* + check for unit id. It would be better if + we had a proper WHOAMI register + */ + if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK && id_high > 0 && + read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK && id_low > 0) { + _unit_id = (uint16_t)((id_high << 8) | id_low) & 0xFFFF; + goto ok; + } + + PX4_DEBUG("probe failed unit_id=0x%02x\n", + (unsigned)_unit_id); + + } else { + /* + 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) { + set_maximum_distance(LL40LS_MAX_DISTANCE_V1); + goto ok; + } + + PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n", + (unsigned)_hw_version, + (unsigned)_sw_version); } - PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n", - (unsigned)_hw_version, - (unsigned)_sw_version); } // not found on any address diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index 3878b2f652..2f7dffb2c9 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -68,6 +68,8 @@ #define LL40LS_SW_VERSION 0x4f #define LL40LS_SIGNAL_STRENGTH_REG 0x0e #define LL40LS_PEAK_STRENGTH_REG 0x0c +#define LL40LS_UNIT_ID_HIGH 0x16 +#define LL40LS_UNIT_ID_LOW 0x17 #define LL40LS_SIG_COUNT_VAL_REG 0x02 /* Maximum acquisition count register */ #define LL40LS_SIG_COUNT_VAL_MAX 0xFF /* Maximum acquisition count max value */ @@ -130,6 +132,7 @@ private: volatile bool _pause_measurements; uint8_t _hw_version; uint8_t _sw_version; + uint16_t _unit_id; /** * LidarLite specific transfer function. This is needed diff --git a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp index d44d9c24bb..89498c22f7 100644 --- a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp +++ b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp @@ -114,6 +114,7 @@ void start(enum LL40LS_BUS busid, uint8_t rotation) if (instance) { PX4_INFO("driver already started"); + return; } if (busid == LL40LS_BUS_PWM) { @@ -149,7 +150,6 @@ void start(enum LL40LS_BUS busid, uint8_t rotation) PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum); stop(); - return; } }