From b7a8b64a6e219b5c0f30e075a1c246db72bc1f40 Mon Sep 17 00:00:00 2001
From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>
Date: Wed, 4 Dec 2024 12:33:42 +0100
Subject: [PATCH] Use Rangefinder

---
 .../lightware_sf45_serial.cpp                  | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
index 53e2c18dbb..4d7ebff901 100755
--- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
@@ -645,17 +645,21 @@ void SF45LaserSerial::sf45_process_replies(float &distance_m)
 				break;
 			}
 
-			// Convert to meters for rangefinder update
-			distance_m = raw_distance * SF45_SCALE_FACTOR;
 
-			Quatf quaternion(Eulerf{0, 0, sf45_wrap_360(scaled_yaw)*M_DEG_TO_RAD_F});
-			float q[4];
+			if (raw_distance < 65436) { // Discard invalid readings
+				// Convert to meters for rangefinder update
+				distance_m = raw_distance * SF45_SCALE_FACTOR;
 
-			for (int i = 0; i < 4; i++) {
-				q[i] = quaternion(i);
+				Quatf quaternion(Eulerf{0, 0, sf45_wrap_360(scaled_yaw)*M_DEG_TO_RAD_F});
+				float q[4];
+
+				for (int i = 0; i < 4; i++) {
+					q[i] = quaternion(i);
+				}
+
+				_px4_rangefinder.update(hrt_absolute_time(), distance_m, -1, q);
 			}
 
-			_px4_rangefinder.update(hrt_absolute_time(), distance_m, -1, q);
 			break;
 		}