From 496e3fe2e06d8cf0acd4b6b9ce28e4a9b6bcf61b Mon Sep 17 00:00:00 2001 From: Sam Chamberlin Date: Wed, 7 Jul 2021 19:40:29 -0600 Subject: [PATCH] afbr-s50: added gate for processing distance measurements updating variable naming convention publish data when measurements are invalid, just set them to zero add quality to distance measurements --- .../broadcom/afbrs50/AFBRS50.cpp | 40 ++++++++++++++----- .../broadcom/afbrs50/AFBRS50.hpp | 3 ++ 2 files changed, 32 insertions(+), 11 deletions(-) diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp index 15bf25d3c0..bac6e46ab5 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -93,7 +93,15 @@ void AFBRS50::ProcessMeasurement(void *data) if ((evaluate_status == STATUS_OK) && (res.Status == 0)) { uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000); float result_m = static_cast(result_mm) / 1000.f; - _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m); + int8_t quality = 100; + + // distance quality check + if (result_m < _min_distance || result_m > _max_distance) { + result_m = 0.0; + quality = 0; + } + + _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality); } } } @@ -137,36 +145,46 @@ int AFBRS50::init() // FALLTHROUGH case AFBR_S50MV85G_V3: - _px4_rangefinder.set_min_distance(0.08f); - _px4_rangefinder.set_max_distance(10.f); + _min_distance = 0.08f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_fov(math::radians(6.f)); PX4_INFO_RAW("AFBR-S50MV85G\n"); break; case AFBR_S50LV85D_V1: - _px4_rangefinder.set_min_distance(0.08f); - _px4_rangefinder.set_max_distance(30.f); + _min_distance = 0.08f; + _max_distance = 30.f; // Short range mode + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_fov(math::radians(6.f)); PX4_INFO_RAW("AFBR-S50LV85D (v1)\n"); break; case AFBR_S50MV68B_V1: - _px4_rangefinder.set_min_distance(0.08f); - _px4_rangefinder.set_max_distance(10.f); + _min_distance = 0.08f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_fov(math::radians(1.f)); PX4_INFO_RAW("AFBR-S50MV68B (v1)\n"); break; case AFBR_S50MV85I_V1: - _px4_rangefinder.set_min_distance(0.08f); - _px4_rangefinder.set_max_distance(5.f); + _min_distance = 0.08f; + _max_distance = 5.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_fov(math::radians(6.f)); PX4_INFO_RAW("AFBR-S50MV85I (v1)\n"); break; case AFBR_S50SV85K_V1: - _px4_rangefinder.set_min_distance(0.08f); - _px4_rangefinder.set_max_distance(10.f); + _min_distance = 0.08f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_fov(math::radians(4.f)); PX4_INFO_RAW("AFBR-S50SV85K (v1)\n"); break; diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp index 651dc709d3..32ad6a8882 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp @@ -89,4 +89,7 @@ private: hrt_abstime _measurement_time{0}; perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")}; + + float _max_distance; + float _min_distance; };