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

ll40ls: fix v3hp signal quality and modified probe to detect v3hp.

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2019-09-04 15:27:16 +01:00
committed by Nuno Marques
parent 327529d063
commit 59f61b3107
2 changed files with 57 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+54 -38
View File
@@ -131,8 +131,8 @@ LidarLiteI2C::probe()
/*
The probing is divided in two cases. One to detect v1, v2 and v3 and one to
detect v3HP. The reason for this is that registers are not consistent
between different versions. The v3HP doesn't have the HW and SW VERSION
registers while v1, v2 and some v3 don't have the unit id register.
between different versions. The v3HP doesn't have the HW VERSION (or at least no version is specified)
while v1, v2 and some v3 don't have the unit id register.
It would be better if we had a proper WHOAMI register.
*/
@@ -142,8 +142,7 @@ LidarLiteI2C::probe()
*/
if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version) > 0 &&
(read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0)) {
_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V1);
printf("probe success - hw: %u, sw:%u\n", _hw_version, _sw_version);
PX4_INFO("probe success - hw: %u, sw:%u", _hw_version, _sw_version);
_retries = 3;
return reset_sensor();
}
@@ -154,12 +153,13 @@ LidarLiteI2C::probe()
if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK &&
read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) {
_unit_id = (uint16_t)((id_high << 8) | id_low) & 0xFFFF;
printf("probe success - id: %u\n", _unit_id);
_is_V3hp = true;
PX4_INFO("probe success - id: %u", _unit_id);
_retries = 3;
return reset_sensor();
}
PX4_DEBUG("probe failed unit_id=0x%02x hw_version=0x%02x sw_version=0x%02x\n",
PX4_DEBUG("probe failed unit_id=0x%02x hw_version=0x%02x sw_version=0x%02x",
(unsigned)_unit_id,
(unsigned)_hw_version,
(unsigned)_sw_version);
@@ -233,7 +233,7 @@ LidarLiteI2C::reset_sensor()
ret = read_reg(LL40LS_SIG_COUNT_VAL_REG, sig_cnt);
if ((ret != OK) || (sig_cnt != LL40LS_SIG_COUNT_VAL_DEFAULT)) {
printf("Error: ll40ls reset failure. Exiting!\n");
PX4_INFO("Error: ll40ls reset failure. Exiting!\n");
return ret;
}
@@ -377,49 +377,65 @@ int LidarLiteI2C::collect()
uint8_t ll40ls_signal_strength = val[0];
uint8_t signal_min = 0;
uint8_t signal_max = 0;
uint8_t signal_value = 0;
/* 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);
// We detect if V3HP is being used
if (_is_V3hp) {
signal_min = LL40LS_SIGNAL_STRENGH_MIN_V3HP;
signal_max = LL40LS_SIGNAL_STRENGH_MAX_V3HP;
signal_value = ll40ls_signal_strength;
// 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 strenght read failed");
} 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);
DEVICE_DEBUG("error reading peak strength from sensor: %d", ret);
perf_count(_comms_errors);
// 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 strenght read failed");
if (perf_event_count(_comms_errors) % 10 == 0) {
perf_count(_sensor_resets);
reset_sensor();
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;
}
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];
signal_min = LL40LS_PEAK_STRENGTH_LOW;
signal_max = LL40LS_PEAK_STRENGTH_HIGH;
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) {
signal_value = 0;
} else {
signal_value = ll40ls_peak_strength;
}
}
/* Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments*/
// Step 1: Normalize signal strength to 0...100 percent using the absolute signal peak strength.
uint8_t signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW,
0) / (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
uint8_t signal_quality = 100 * math::max(signal_value - signal_min, 0) / (signal_max - signal_min);
// Step 2: Also 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) {
signal_quality = 0;
}
// Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
// Step 2: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
if (distance_m < LL40LS_MIN_DISTANCE) {
signal_quality = 0;
}
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h
+3
View File
@@ -67,6 +67,8 @@ static constexpr uint8_t LL40LS_UNIT_ID_LOW = 0x17;
static constexpr uint8_t LL40LS_SIG_COUNT_VAL_REG = 0x02; /* Maximum acquisition count register */
static constexpr uint8_t LL40LS_SIG_COUNT_VAL_MAX = 0xFF; /* Maximum acquisition count max value */
static constexpr int LL40LS_SIGNAL_STRENGH_MIN_V3HP = 70; // Min signal strength for V3HP
static constexpr int LL40LS_SIGNAL_STRENGH_MAX_V3HP = 255; // Max signal strength for V3HP
static constexpr int LL40LS_SIGNAL_STRENGTH_LOW =
24; // Minimum (relative) signal strength value for accepting a measurement
@@ -148,5 +150,6 @@ private:
uint8_t _hw_version{0};
uint8_t _sw_version{0};
uint16_t _unit_id{0};
bool _is_V3hp{false};
};