fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 06:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ll40ls: add support for LidarLite V4

Browse Source 
Co-authored-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Martina Rivizzigno
2020-10-26 09:16:21 +01:00
committed by Daniel Agar
parent 68dcc25709
commit 159c87a6fb
2 changed files with 131 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+113 -63
View File
@@ -140,8 +140,35 @@ LidarLiteI2C::probe()
* v1 and v3 don't have the unit id register while v2 has both.
* It would be better if we had a proper WHOAMI register.
*/
if (read_reg(LL40LS_HW_VERSION_V4, _hw_version) == OK &&
read_reg(LL40LS_SW_VERSION_V4, _sw_version) == OK) {
if (_hw_version > 0) {
PX4_DEBUG("probe success - hw: %u, sw: %u", _hw_version, _sw_version);
uint8_t id_byte_0 = 0;
uint8_t id_byte_1 = 0;
uint8_t id_byte_2 = 0;
uint8_t id_byte_3 = 0;
if (read_reg(LL40LS_UNIT_ID_0_V4, id_byte_0) == OK &&
read_reg(LL40LS_UNIT_ID_1_V4, id_byte_1) == OK &&
read_reg(LL40LS_UNIT_ID_2_V4, id_byte_2) == OK &&
read_reg(LL40LS_UNIT_ID_3_V4, id_byte_3) == OK) {
_unit_id = ((uint32_t)id_byte_3 << 24) | ((uint32_t)id_byte_2 << 16) | ((uint32_t)id_byte_1 << 8) |
(uint32_t)id_byte_0;
}
_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V4);
_model = Model::v4;
return OK;
}
}
if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK) &&
(read_reg(LL40LS_SW_VERSION, _sw_version) == OK)) {
PX4_DEBUG("hw: %u - sw %u \n", _hw_version, _sw_version);
if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK &&
read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) {
@@ -152,20 +179,20 @@ LidarLiteI2C::probe()
if (_unit_id > 0) {
// v2
PX4_INFO("probe success - hw: %" PRIu8 ", sw:%" PRIu8 ", id: %" PRIu16, _hw_version, _sw_version, _unit_id);
PX4_DEBUG("probe success - hw: %" PRIu8 ", sw:%" PRIu8 ", id: %" PRIu16, _hw_version, _sw_version, _unit_id);
_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V2);
} else {
// v1 and v3
PX4_INFO("probe success - hw: %" PRIu8 ", sw:%" PRIu8, _hw_version, _sw_version);
PX4_DEBUG("probe success - hw: %" PRIu8 ", sw:%" PRIu8, _hw_version, _sw_version);
}
} else {
if (_unit_id > 0) {
// v3hp
_is_v3hp = true;
PX4_INFO("probe success - id: %" PRIu16, _unit_id);
_model = Model::v3hp;
PX4_DEBUG("probe success - id: %" PRIu16, _unit_id);
}
}
@@ -289,18 +316,29 @@ LidarLiteI2C::print_registers()
int
LidarLiteI2C::collect()
{
// read from the sensor
uint8_t val[2] {};
int ret = 0;
perf_begin(_sample_perf);
uint8_t invalid_distance_index = 0;
// read the high and low byte distance registers
uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
int ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
if (_model == Model::v4) {
uint8_t distance_reg = LL40LS_DISTHIGH_REG_V4;
ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
invalid_distance_index = 1;
} else {
uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
}
// 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 (ret < 0 || (val[invalid_distance_index] & 0x80)) {
if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
/*
NACKs from the sensor are expected when we
@@ -322,7 +360,15 @@ LidarLiteI2C::collect()
return ret;
}
uint16_t distance_cm = (val[0] << 8) | val[1];
uint16_t distance_cm = 0;
if (_model == Model::v4) {
distance_cm = ((uint16_t)(val[1] << 8)) | (uint16_t)val[0];
} else {
distance_cm = (val[0] << 8) | val[1];
}
const float distance_m = float(distance_cm) * 1e-2f;
if (distance_cm == 0) {
@@ -349,61 +395,25 @@ LidarLiteI2C::collect()
// this should be fairly close to the end of the measurement, so the best approximation of the time
const hrt_abstime timestamp_sample = hrt_absolute_time();
// Relative signal strength measurement, i.e. the strength of
// the main signal peak compared to the general noise level.
uint8_t signal_strength_reg = LL40LS_SIGNAL_STRENGTH_REG;
ret = lidar_transfer(&signal_strength_reg, 1, &val[0], 1);
int8_t signal_quality = -1;
// check if the transfer failed
if (ret < 0) {
if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) {
/*
NACKs from the sensor are expected when we
read before it is ready, so only consider it
an error if more than 100ms has elapsed.
*/
PX4_INFO("signal strength read failed");
DEVICE_DEBUG("error reading signal strength from sensor: %d", ret);
perf_count(_comms_errors);
if (perf_event_count(_comms_errors) % 10 == 0) {
perf_count(_sensor_resets);
reset_sensor();
}
}
perf_end(_sample_perf);
// if we are getting lots of I2C transfer errors try
// resetting the sensor
return ret;
}
uint8_t ll40ls_signal_strength = val[0];
uint8_t signal_quality;
// We detect if V3HP is being used
if (_is_v3hp) {
//Normalize signal strength to 0...100 percent using the absolute signal strength.
signal_quality = 100 * math::max(ll40ls_signal_strength - LL40LS_SIGNAL_STRENGTH_MIN_V3HP, 0) /
(LL40LS_SIGNAL_STRENGTH_MAX_V3HP - LL40LS_SIGNAL_STRENGTH_MIN_V3HP);
} else {
// Absolute peak strength measurement, i.e. absolute strength of main signal peak.
uint8_t peak_strength_reg = LL40LS_PEAK_STRENGTH_REG;
ret = lidar_transfer(&peak_strength_reg, 1, &val[0], 1);
if (_model != Model::v4) {
// Relative signal strength measurement, i.e. the strength of
// the main signal peak compared to the general noise level.
uint8_t signal_strength_reg = LL40LS_SIGNAL_STRENGTH_REG;
ret = lidar_transfer(&signal_strength_reg, 1, &val[0], 1);
// check if the transfer failed
if (ret < 0) {
if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) {
/*
NACKs from the sensor are expected when we
read before it is ready, so only consider it
an error if more than 100ms has elapsed.
*/
PX4_INFO("peak strength read failed");
NACKs from the sensor are expected when we
read before it is ready, so only consider it
an error if more than 100ms has elapsed.
*/
PX4_INFO("signal strength read failed");
DEVICE_DEBUG("error reading peak strength from sensor: %d", ret);
DEVICE_DEBUG("error reading signal strength from sensor: %d", ret);
perf_count(_comms_errors);
if (perf_event_count(_comms_errors) % 10 == 0) {
@@ -418,18 +428,58 @@ LidarLiteI2C::collect()
return ret;
}
uint8_t ll40ls_peak_strength = val[0];
uint8_t ll40ls_signal_strength = val[0];
// For v2 and v3 use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements
if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW || distance_m < LL40LS_MIN_DISTANCE) {
signal_quality = 0;
// We detect if V3HP is being used
if (_model == Model::v3hp) {
//Normalize signal strength to 0...100 percent using the absolute signal strength.
signal_quality = 100 * math::max(ll40ls_signal_strength - LL40LS_SIGNAL_STRENGTH_MIN_V3HP, 0) /
(LL40LS_SIGNAL_STRENGTH_MAX_V3HP - LL40LS_SIGNAL_STRENGTH_MIN_V3HP);
} else {
//Normalize signal strength to 0...100 percent using the absolute signal peak strength.
signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW, 0) /
(LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
// Absolute peak strength measurement, i.e. absolute strength of main signal peak.
uint8_t peak_strength_reg = LL40LS_PEAK_STRENGTH_REG;
ret = lidar_transfer(&peak_strength_reg, 1, &val[0], 1);
// check if the transfer failed
if (ret < 0) {
if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) {
/*
NACKs from the sensor are expected when we
read before it is ready, so only consider it
an error if more than 100ms has elapsed.
*/
PX4_INFO("peak strength read failed");
DEVICE_DEBUG("error reading peak strength from sensor: %d", ret);
perf_count(_comms_errors);
if (perf_event_count(_comms_errors) % 10 == 0) {
perf_count(_sensor_resets);
reset_sensor();
}
}
perf_end(_sample_perf);
// if we are getting lots of I2C transfer errors try
// resetting the sensor
return ret;
}
uint8_t ll40ls_peak_strength = val[0];
// For v2 and v3 use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements
if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW || distance_m < LL40LS_MIN_DISTANCE) {
signal_quality = 0;
} else {
//Normalize signal strength to 0...100 percent using the absolute signal peak strength.
signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW, 0) /
(LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
}
}
}
_px4_rangefinder.update(timestamp_sample, distance_m, signal_quality);
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h
+18 -2
View File
@@ -83,6 +83,15 @@ static constexpr int LL40LS_PEAK_STRENGTH_HIGH = 234; /* Max peak strengt
static constexpr float LL40LS_MIN_DISTANCE{0.05f};
static constexpr float LL40LS_MAX_DISTANCE{25.00f};
static constexpr float LL40LS_MAX_DISTANCE_V2{35.00f};
static constexpr float LL40LS_MAX_DISTANCE_V4{10.00f};
static constexpr uint8_t LL40LS_HW_VERSION_V4 = 0xE1;
static constexpr uint8_t LL40LS_SW_VERSION_V4 = 0x30;
static constexpr uint8_t LL40LS_UNIT_ID_0_V4 = 0x16;
static constexpr uint8_t LL40LS_UNIT_ID_1_V4 = 0x17;
static constexpr uint8_t LL40LS_UNIT_ID_2_V4 = 0x18;
static constexpr uint8_t LL40LS_UNIT_ID_3_V4 = 0x19;
static constexpr uint8_t LL40LS_DISTHIGH_REG_V4 = 0x10; /* High byte of distance register, auto increment */
// Normal conversion wait time.
static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{50_ms};
@@ -168,13 +177,20 @@ private:
int probe_address(const uint8_t address);
bool _collect_phase{false};
bool _is_v3hp{false};
bool _pause_measurements{false};
enum class Model {
Generic = 0,
v3hp = 1,
v4 = 2,
};
Model _model{Model::Generic};
uint8_t _hw_version{0};
uint8_t _sw_version{0};
uint16_t _unit_id{0};
uint32_t _unit_id{0};
uint16_t _zero_counter{0};
uint64_t _acquire_time_usec{0};