fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 09:43:34 +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-07-28 23:45:38 +01:00
committed by Nuno Marques
parent 4d1c7b734c
commit 8a3a71caed
1 changed files with 16 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+16 -31
View File
@@ -219,7 +219,7 @@ LidarLiteI2C::measure()
int int
LidarLiteI2C::reset_sensor() LidarLiteI2C::reset_sensor()
{ {
px4_usleep(15000); px4_usleep(15_ms);
int ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX); int ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX);
@@ -227,14 +227,14 @@ LidarLiteI2C::reset_sensor()
return ret; return ret;
} }
px4_usleep(15000); px4_usleep(15_ms);
ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
if (ret != PX4_OK) { if (ret != PX4_OK) {
uint8_t sig_cnt; uint8_t sig_cnt;
px4_usleep(15000); px4_usleep(15_ms);
ret = read_reg(LL40LS_SIG_COUNT_VAL_REG, sig_cnt); ret = read_reg(LL40LS_SIG_COUNT_VAL_REG, sig_cnt);
if ((ret != PX4_OK) || (sig_cnt != LL40LS_SIG_COUNT_VAL_DEFAULT)) { if ((ret != PX4_OK) || (sig_cnt != LL40LS_SIG_COUNT_VAL_DEFAULT)) {
@@ -245,7 +245,7 @@ LidarLiteI2C::reset_sensor()
} }
// wait for sensor reset to complete // 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); ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX);
if (ret != PX4_OK) { if (ret != PX4_OK) {
@@ -253,7 +253,7 @@ LidarLiteI2C::reset_sensor()
} }
// wait for register write to complete // wait for register write to complete
px4_usleep(1000); px4_usleep(1_ms);
return OK; return OK;
} }
@@ -264,7 +264,7 @@ LidarLiteI2C::print_registers()
_pause_measurements = true; _pause_measurements = true;
PX4_INFO("registers"); PX4_INFO("registers");
// wait for a while to ensure the lidar is in a ready state // 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++) { for (uint8_t reg = 0; reg <= 0x67; reg++) {
uint8_t val = 0; uint8_t val = 0;
@@ -380,16 +380,13 @@ LidarLiteI2C::collect()
} }
uint8_t ll40ls_signal_strength = val[0]; uint8_t ll40ls_signal_strength = val[0];
uint8_t signal_quality;
uint8_t signal_min = 0;
uint8_t signal_max = 0;
uint8_t signal_value = 0;
// We detect if V3HP is being used // We detect if V3HP is being used
if (_is_v3hp) { if (_is_v3hp) {
signal_min = LL40LS_SIGNAL_STRENGTH_MIN_V3HP; //Normalize signal strength to 0...100 percent using the absolute signal strength.
signal_max = LL40LS_SIGNAL_STRENGTH_MAX_V3HP; signal_quality = 100 * math::max(ll40ls_signal_strength - LL40LS_SIGNAL_STRENGTH_MIN_V3HP, 0) /
signal_value = ll40ls_signal_strength; (LL40LS_SIGNAL_STRENGTH_MAX_V3HP - LL40LS_SIGNAL_STRENGTH_MIN_V3HP);
} else { } else {
// Absolute peak strength measurement, i.e. absolute strength of main signal peak. // 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]; 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 // 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) { if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW || distance_m < LL40LS_MIN_DISTANCE) {
signal_value = 0; signal_quality = 0;
} else { } 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); _px4_rangefinder.update(timestamp_sample, distance_m, signal_quality);
perf_end(_sample_perf); perf_end(_sample_perf);