diff --git a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp
index 2c3b22d57b..9ca071e0ab 100644
--- a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp
+++ b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp
@@ -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);
diff --git a/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp b/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp
index 2161bcf5b3..52745457d7 100644
--- a/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp
+++ b/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp
@@ -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")};