fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 02:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ll40ls driver: support also lidar lite v3HP (#10578)

Browse Source
This commit is contained in:
Daniele Pettenuzzo
2018-10-04 00:54:27 -04:00
committed by Daniel Agar
parent 6f7c572601
commit a2d1f6ddce
5 changed files with 48 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+40 -11
View File
@@ -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