fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 16:48:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

afbr-s50: added gate for processing distance measurements

Browse Source 
updating variable naming convention

publish data when measurements are invalid, just set them to zero

add quality to distance measurements
This commit is contained in:
Sam Chamberlin
2021-07-07 21:40:29 -04:00
committed by GitHub
parent dfd6999643
commit 496e3fe2e0
2 changed files with 32 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+29 -11
View File
@@ -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<float>(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;
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp
+3
View File
@@ -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;
};