diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index bf9db74e7f..70872f5076 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -5,6 +5,7 @@ float32 min_distance # Minimum distance the sensor can measure (in m) float32 max_distance # Maximum distance the sensor can measure (in m) float32 current_distance # Current distance reading (in m) float32 covariance # Measurement covariance (in m), 0 for unknown / invalid readings +int8 signal_strength # Signal strength in percent (0...100%) or -1 if unknown uint8 type # Type from MAV_DISTANCE_SENSOR enum uint8 MAV_DISTANCE_SENSOR_LASER = 0 diff --git a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp index d7584c1505..0d28a8f1c3 100644 --- a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp +++ b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp @@ -411,6 +411,7 @@ void leddar_one::_publish(uint16_t distance_mm) report.min_distance = MIN_DISTANCE; report.max_distance = MAX_DISTANCE; report.covariance = 0.0f; + report.signal_strength = -1; report.id = 0; _reports->force(&report); diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index 8955a6e7de..d280988c9a 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int address) : @@ -448,6 +449,80 @@ int LidarLiteI2C::collect() _last_distance = distance_cm; + /* 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_absolute_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]; + + + /* 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_absolute_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"); + + 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]; + + /* 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_strength = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW, + 0) / (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW); + + // 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_strength = 0; } + + // Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS. + if (distance_m < LL40LS_MIN_DISTANCE) { signal_strength = 0; } /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); @@ -455,11 +530,11 @@ int LidarLiteI2C::collect() report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; - /* the sensor is in fact a laser + sonar but there is no enum for this */ - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.signal_strength = signal_strength; + report.type = + distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; // the sensor is in fact a laser + sonar but there is no enum for this report.orientation = _rotation; - /* TODO: set proper ID */ - report.id = 0; + report.id = 0; // TODO: set proper ID /* publish it, if we are the primary */ if (_distance_sensor_topic != nullptr) { diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index bd18d00ec1..3878b2f652 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -66,11 +66,16 @@ #define LL40LS_AUTO_INCREMENT 0x80 #define LL40LS_HW_VERSION 0x41 #define LL40LS_SW_VERSION 0x4f -#define LL40LS_SIGNAL_STRENGTH_REG 0x5b +#define LL40LS_SIGNAL_STRENGTH_REG 0x0e +#define LL40LS_PEAK_STRENGTH_REG 0x0c #define LL40LS_SIG_COUNT_VAL_REG 0x02 /* Maximum acquisition count register */ #define LL40LS_SIG_COUNT_VAL_MAX 0xFF /* Maximum acquisition count max value */ +#define LL40LS_SIGNAL_STRENGTH_LOW 24 // Minimum (relative) signal strength value for accepting a measurement +#define LL40LS_PEAK_STRENGTH_LOW 135 // Minimum peak strength raw value for accepting a measurement +#define LL40LS_PEAK_STRENGTH_HIGH 234 // Max peak strength raw value + class LidarLiteI2C : public LidarLite, public device::I2C { public: diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index c9e599f83f..c437c52198 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -556,6 +556,7 @@ MB12XX::collect() report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + report.signal_strength = -1; /* TODO: set proper ID */ report.id = 0; diff --git a/src/drivers/distance_sensor/sf0x/sf0x.cpp b/src/drivers/distance_sensor/sf0x/sf0x.cpp index 3d6cb3dc8d..59700d039e 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x.cpp +++ b/src/drivers/distance_sensor/sf0x/sf0x.cpp @@ -616,6 +616,7 @@ SF0X::collect() report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + report.signal_strength = -1; /* TODO: set proper ID */ report.id = 0; diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 540dcd63b0..4806433842 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -552,6 +552,7 @@ SF1XX::collect() report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + report.signal_strength = -1; /* TODO: set proper ID */ report.id = 0; diff --git a/src/drivers/distance_sensor/srf02/srf02.cpp b/src/drivers/distance_sensor/srf02/srf02.cpp index 105769d6ee..ea246eb44a 100644 --- a/src/drivers/distance_sensor/srf02/srf02.cpp +++ b/src/drivers/distance_sensor/srf02/srf02.cpp @@ -559,6 +559,7 @@ SRF02::collect() report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + report.signal_strength = -1; /* TODO: set proper ID */ report.id = 0; diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp index c526b16e93..8959c752c1 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp @@ -633,6 +633,7 @@ TERARANGER::collect() report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + report.signal_strength = -1; /* TODO: set proper ID */ report.id = 0; diff --git a/src/drivers/distance_sensor/tfmini/tfmini.cpp b/src/drivers/distance_sensor/tfmini/tfmini.cpp index 0d436bd039..7862ab1136 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini.cpp @@ -580,6 +580,7 @@ TFMINI::collect() report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + report.signal_strength = -1; /* TODO: set proper ID */ report.id = 0; diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index f8c53ccbb5..5dcd021c02 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -675,6 +675,7 @@ VL53LXX::collect() report.min_distance = VL53LXX_MIN_RANGING_DISTANCE; report.max_distance = VL53LXX_MAX_RANGING_DISTANCE; report.covariance = 0.0f; + report.signal_strength = -1; /* TODO: set proper ID */ report.id = 0; diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 5fd7657799..edc06c2e42 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -595,6 +595,7 @@ PX4FLOW::collect() distance_report.max_distance = PX4FLOW_MAX_DISTANCE; distance_report.current_distance = report.ground_distance_m; distance_report.covariance = 0.0f; + distance_report.signal_strength = -1; distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; /* TODO: the ID needs to be properly set */ distance_report.id = 0; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index ab665aa90e..8a04fc89c5 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -941,7 +941,8 @@ void Ekf2::run() if (orb_copy(ORB_ID(distance_sensor), _range_finder_subs[_range_finder_sub_index], &range_finder) == PX4_OK) { // check if distance sensor is within working boundaries if (range_finder.min_distance >= range_finder.current_distance || - range_finder.max_distance <= range_finder.current_distance) { + range_finder.max_distance <= range_finder.current_distance || + range_finder.signal_strength == 0) { // use rng_gnd_clearance if on ground if (_ekf.get_in_air_status()) { range_finder_updated = false; @@ -951,7 +952,7 @@ void Ekf2::run() } } - _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance); + if (range_finder_updated) { _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance); } // Save sensor limits reported by the rangefinder _ekf.set_rangefinder_limits(range_finder.min_distance, range_finder.max_distance); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 0214d00d35..f9b3988ae5 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1173,6 +1173,7 @@ int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink) dist.id = dist_mavlink->id; dist.orientation = dist_mavlink->orientation; dist.covariance = dist_mavlink->covariance / 100.0f; + dist.signal_strength = -1; int dist_multi; orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH); diff --git a/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp b/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp index 4f171bdae4..9d7b84e2b9 100644 --- a/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp +++ b/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp @@ -162,6 +162,8 @@ int DfBebopRangeFinderWrapper::_publish(struct bebop_range &data) distance_data.covariance = 1.0f; // TODO set correct value + distance_data.signal_strength = -1; + if (_range_topic == nullptr) { _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &distance_data, &_orb_class_instance, ORB_PRIO_DEFAULT); diff --git a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp index 5ab60782dd..f84fea3308 100644 --- a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp +++ b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp @@ -169,6 +169,8 @@ int DfISL29501Wrapper::_publish(struct range_sensor_data &data) d.covariance = 0.0f; + d.signal_strength = -1; + if (_range_topic == nullptr) { _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, &_orb_class_instance, ORB_PRIO_DEFAULT); diff --git a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp index cdd2f5e682..83b3fa0888 100644 --- a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp +++ b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp @@ -175,6 +175,8 @@ int DfTROneWrapper::_publish(struct range_sensor_data &data) d.covariance = 0.0f; + d.signal_strength = -1; + if (_range_topic == nullptr) { _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, &_orb_class_instance, ORB_PRIO_DEFAULT);