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

simplify the code to calculate signal_quality

Browse Source
This commit is contained in:
bazooka joe 2020-06-18 01:11:40 -07:00 committed by Nuno Marques
parent 4d1c7b734c
commit 8a3a71caed
1 changed files with 16 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
View File

@ -219,7 +219,7 @@ LidarLiteI2C::measure()
int
LidarLiteI2C::reset_sensor()
{
px4_usleep(15000);
px4_usleep(15_ms);
int ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX);
@ -227,14 +227,14 @@ LidarLiteI2C::reset_sensor()
return ret;
}
px4_usleep(15000);
px4_usleep(15_ms);
ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
if (ret != PX4_OK) {
uint8_t sig_cnt;
px4_usleep(15000);
px4_usleep(15_ms);
ret = read_reg(LL40LS_SIG_COUNT_VAL_REG, sig_cnt);
if ((ret != PX4_OK) || (sig_cnt != LL40LS_SIG_COUNT_VAL_DEFAULT)) {
@ -245,7 +245,7 @@ LidarLiteI2C::reset_sensor()
}
// wait for sensor reset to complete
px4_usleep(50000);
px4_usleep(50_ms);
ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX);
if (ret != PX4_OK) {
@ -253,7 +253,7 @@ LidarLiteI2C::reset_sensor()
}
// wait for register write to complete
px4_usleep(1000);
px4_usleep(1_ms);
return OK;
}
@ -264,7 +264,7 @@ LidarLiteI2C::print_registers()
_pause_measurements = true;
PX4_INFO("registers");
// wait for a while to ensure the lidar is in a ready state
px4_usleep(50000);
px4_usleep(50_ms);
for (uint8_t reg = 0; reg <= 0x67; reg++) {
uint8_t val = 0;
@ -380,16 +380,13 @@ LidarLiteI2C::collect()
}
uint8_t ll40ls_signal_strength = val[0];
uint8_t signal_min = 0;
uint8_t signal_max = 0;
uint8_t signal_value = 0;
uint8_t signal_quality;
// We detect if V3HP is being used
if (_is_v3hp) {
signal_min = LL40LS_SIGNAL_STRENGTH_MIN_V3HP;
signal_max = LL40LS_SIGNAL_STRENGTH_MAX_V3HP;
signal_value = ll40ls_signal_strength;
//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.
@ -422,31 +419,19 @@ LidarLiteI2C::collect()
}
uint8_t ll40ls_peak_strength = val[0];
signal_min = LL40LS_PEAK_STRENGTH_LOW;
signal_max = LL40LS_PEAK_STRENGTH_HIGH;
// 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;
if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW || distance_m < LL40LS_MIN_DISTANCE) {
signal_quality = 0;
} else {
signal_value = ll40ls_peak_strength;
//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);
}
}
uint8_t signal_quality;
if (!_is_v3hp && distance_m < LL40LS_MIN_DISTANCE) {
//for Lidar lites that are not v3hp we need to filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
signal_quality = 0;
} else {
//Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments
//Normalize signal strength to 0...100 percent using the absolute signal peak strength.
signal_quality = 100 * math::max(signal_value - signal_min, 0) / (signal_max - signal_min);
}
_px4_rangefinder.update(timestamp_sample, distance_m, signal_quality);
perf_end(_sample_perf);