Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

fix(drivers/distance_sensor/vl53l1x): map rangestatus to quality (#27308)

Browse Source 
* fix(drivers/distance_sensor/vl53l1x): added median filtering and hysteresis based quality

* fix(drivers/distance_sensor/vl53l1x): revert back to 100/0 quality
This commit is contained in:
Himanshu
2026-05-19 03:23:17 +05:45
committed by GitHub
parent 2d321addec
commit ccedd9d87e
2 changed files with 28 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp
+14 -4
View File
@@ -42,7 +42,6 @@
#define VL53L1X_INTER_MEAS_MS 250 // ms
#define VL53L1X_SHORT_RANGE 1 // sub-2 meter distance mode
#define VL53L1X_LONG_RANGE 2 // sub-4 meter distance mode
#define VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS 13 // region of interest out of bounds error
#define VL53L1X_RANGE_STATUS_OK 0 // range status ok
#define VL53L1X_ROI_MID_RIGHT 231 // ROI middle right of optical center
#define VL53L1X_ROI_MID_LEFT 167 // ROI middle left of optical center
@@ -197,7 +196,7 @@ VL53L1X::~VL53L1X()
int VL53L1X::collect()
{
uint8_t ret = 0;
uint8_t rangeStatus = VL53L1X_RANGE_STATUS_OK;
uint8_t raw_status = VL53L1X_RANGE_STATUS_OK;
uint16_t distance_mm = 0;
static constexpr float DOWNWARD_FACING_THETA = -1.57f;
static constexpr float RIGHT_PSI = 0.117809725;
@@ -205,14 +204,25 @@ int VL53L1X::collect()
perf_begin(_sample_perf);
const int8_t quality = VL53L1X_GetRangeStatus(&rangeStatus);
ret = VL53L1X_GetRangeStatus(&raw_status);
if ((ret != PX4_OK) | (rangeStatus == VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS)) {
RangeStatus rangeStatus = static_cast<RangeStatus>(raw_status);
if ((ret != PX4_OK) || (rangeStatus == RangeStatus::RoiOutOfBounds)) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return PX4_ERROR;
}
int8_t quality = 0;
if (rangeStatus == RangeStatus::Ok) {
quality = 100;
} else {
quality = 0;
}
const hrt_abstime timestamp_sample = hrt_absolute_time();
ret = VL53L1X_GetDistance(&distance_mm);
src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp
+14 -2
View File
@@ -33,8 +33,9 @@
/**
* @file vl53l1x.hpp
*
* Driver for the ST VL53L1X ToF Sensor connected via I2C.
* Datasheet: https://www.st.com/resource/en/datasheet/vl53l1x.pdf
* API User Manual: https://www.st.com/resource/en/user_manual/um2356-vl53l1x-api-user-manual-stmicroelectronics.pdf
*/
#pragma once
@@ -48,6 +49,7 @@
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <lib/mathlib/math/filter/MedianFilter.hpp>
#include <matrix/math.hpp>
/* ST */
#define SOFT_RESET 0x0000
@@ -101,7 +103,16 @@ class VL53L1X : public device::I2C, public I2CSPIDriver<VL53L1X>
{
public:
VL53L1X(const I2CSPIDriverConfig &config);
enum class RangeStatus : uint8_t {
Ok = 0,
SigmaFail = 1,
SignalFail = 2,
OutOfBounds = 4,
HardwareFail = 5,
Wraparound = 7,
ProcessingFail = 8,
RoiOutOfBounds = 14
};
~VL53L1X() override;
static void print_usage();
@@ -156,6 +167,7 @@ private:
int8_t VL53L1X_SetInterMeasurementInMs(uint32_t InterMeasurementInMs);
int8_t VL53L1X_SetROICenter(uint8_t data);
int8_t VL53L1X_SetROI(uint16_t x, uint16_t y);
PX4Rangefinder _px4_rangefinder;
Rotation _sensor_orientation{ROTATION_PITCH_270};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};