Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

ll40ls: added support for 'blue label' lidars

Browse Source 
the key is splitting up read_reg() into two transfers
This commit is contained in:
Andrew Tridgell
2015-08-15 19:13:07 +10:00
committed by Lorenz Meier
parent 210bb9e996
commit 41b3452e22
2 changed files with 72 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ll40ls/LidarLiteI2C.cpp
+52 -32
View File
@@ -72,6 +72,8 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
_zero_counter(0),
_acquire_time_usec(0),
_pause_measurements(false),
_hw_version(0),
_sw_version(0),
_bus(bus)
{
// up the retries since the device misses the first measure attempts
@@ -146,7 +148,30 @@ out:
int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val)
{
return transfer(&reg, 1, &val, 1);
return lidar_transfer(&reg, 1, &val, 1);
}
int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val)
{
const uint8_t cmd[2] = { reg, val };
return transfer(&cmd[0], 2, NULL, 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)
{
if (send != NULL && send_len > 0) {
int ret = transfer(send, send_len, NULL, 0);
if (ret != OK)
return ret;
}
if (recv != NULL && recv_len > 0) {
return transfer(NULL, 0, recv, recv_len);
}
return OK;
}
int LidarLiteI2C::probe()
@@ -158,28 +183,18 @@ int LidarLiteI2C::probe()
_retries = 10;
for (uint8_t i = 0; i < sizeof(addresses); i++) {
uint8_t who_am_i = 0, max_acq_count = 0;
// set the I2C bus address
set_address(addresses[i]);
/* register 2 defaults to 0x80. If this matches it is
almost certainly a ll40ls */
if (read_reg(LL40LS_MAX_ACQ_COUNT_REG, max_acq_count) == OK && max_acq_count == 0x80) {
// very likely to be a ll40ls. This is the
// default max acquisition counter
/*
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;
}
if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) {
// it is responding correctly to a
// WHO_AM_I. This works with older sensors (pre-production)
goto ok;
}
DEVICE_DEBUG("probe failed reg11=0x%02x reg2=0x%02x\n",
(unsigned)who_am_i,
(unsigned)max_acq_count);
DEVICE_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n",
(unsigned)_hw_version,
(unsigned)_sw_version);
}
// not found on any address
@@ -187,9 +202,6 @@ int LidarLiteI2C::probe()
ok:
_retries = 3;
// reset the sensor to ensure it is in a known state with
// correct settings
return reset_sensor();
}
@@ -303,7 +315,7 @@ int LidarLiteI2C::measure()
* Send the command to begin a measurement.
*/
const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
ret = transfer(cmd, sizeof(cmd), nullptr, 0);
ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
@@ -332,9 +344,14 @@ int LidarLiteI2C::measure()
*/
int LidarLiteI2C::reset_sensor()
{
const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET };
int ret = transfer(cmd, sizeof(cmd), nullptr, 0);
return ret;
int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
if (ret != OK)
return ret;
// wait for sensor reset to complete
usleep(1000);
return OK;
}
/*
@@ -349,7 +366,7 @@ void LidarLiteI2C::print_registers()
for (uint8_t reg = 0; reg <= 0x67; reg++) {
uint8_t val = 0;
int ret = transfer(&reg, 1, &val, 1);
int ret = lidar_transfer(&reg, 1, &val, 1);
if (ret != OK) {
printf("%02x:XX ", (unsigned)reg);
@@ -377,10 +394,12 @@ int LidarLiteI2C::collect()
perf_begin(_sample_perf);
// read the high and low byte distance registers
uint8_t distance_reg = LL40LS_DISTHIGH_REG;
ret = transfer(&distance_reg, 1, &val[0], sizeof(val));
uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
if (ret < 0) {
// if the transfer failed or if the high bit of distance is
// set then the distance is invalid
if (ret < 0 || (val[0] & 0x80)) {
if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
/*
NACKs from the sensor are expected when we
@@ -427,7 +446,8 @@ int LidarLiteI2C::collect()
_zero_counter = 0;
}
_last_distance = distance_m;
_last_distance = distance_cm;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
src/drivers/ll40ls/LidarLiteI2C.h
+20 -4
View File
@@ -63,10 +63,10 @@
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */
#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_MAX_ACQ_COUNT_REG 0x02 /* maximum acquisition count register */
#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */
#define LL40LS_WHO_AM_I_REG 0x11
#define LL40LS_WHO_AM_I_REG_VAL 0xCA
#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */
#define LL40LS_AUTO_INCREMENT 0x80
#define LL40LS_HW_VERSION 0x41
#define LL40LS_SW_VERSION 0x4f
#define LL40LS_SIGNAL_STRENGTH_REG 0x5b
class LidarLiteI2C : public LidarLite, public device::I2C
@@ -93,6 +93,7 @@ public:
protected:
int probe();
int read_reg(uint8_t reg, uint8_t &val);
int write_reg(uint8_t reg, uint8_t val);
int measure() override;
int reset_sensor();
@@ -116,10 +117,25 @@ private:
uint16_t _zero_counter;
uint64_t _acquire_time_usec;
volatile bool _pause_measurements;
uint8_t _hw_version;
uint8_t _sw_version;
/**< the bus the device is connected to */
int _bus;
/**
* LidarLite specific transfer function. This is needed
* to avoid a stop transition with SCL high
*
* @param send Pointer to bytes to send.
* @param send_len Number of bytes to send.
* @param recv Pointer to buffer for bytes received.
* @param recv_len Number of bytes to receive.
* @return OK if the transfer was successful, -errno
* otherwise.
*/
int lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
/**
* Test whether the device supported by the driver is present at a
* specific address.