From c5834b29ec8bb455351e26b49b4963918f778326 Mon Sep 17 00:00:00 2001
From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>
Date: Tue, 5 Nov 2024 15:11:11 +0100
Subject: [PATCH] added handling of -1 as out of range indicator for 1D
 distance sensors

---
 .../CollisionPrevention.cpp                   |  5 ++++
 .../CollisionPrevention.hpp                   |  3 +-
 .../CollisionPreventionTest.cpp               | 28 +++++++++++++++++++
 3 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp
index f4b25807a1bf..df3179cf6cec 100644
--- a/src/lib/collision_prevention/CollisionPrevention.cpp
+++ b/src/lib/collision_prevention/CollisionPrevention.cpp
@@ -310,6 +310,11 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 	// clamp at maximum sensor range
 	float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance);
 
+	// negative values indicate out of range but valid measurements.
+	if ((distance_sensor.current_distance - -1.f) < FLT_EPSILON && distance_sensor.signal_quality == 0) {
+		distance_reading = distance_sensor.max_distance;
+	}
+
 	// discard values below min range
 	if ((distance_reading > distance_sensor.min_distance)) {
 
diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp
index eb592d62eeb3..72abf96c7546 100644
--- a/src/lib/collision_prevention/CollisionPrevention.hpp
+++ b/src/lib/collision_prevention/CollisionPrevention.hpp
@@ -85,6 +85,7 @@ class CollisionPrevention : public ModuleParams
 	 * @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed
 	 */
 	void modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel);
+
 protected:
 
 	obstacle_distance_s _obstacle_map_body_frame {};
@@ -116,7 +117,7 @@ class CollisionPrevention : public ModuleParams
 
 	/**
 	 * Constrain the acceleration setpoint based on the distance to the obstacle
-	 *  The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above
+	 * The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above
 	 *           +1          ________ _ _
 	 * ┌─┐      │           //
 	 * │X│      │          //
diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp
index 24d5f37894bb..bfb587739f18 100644
--- a/src/lib/collision_prevention/CollisionPreventionTest.cpp
+++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp
@@ -617,6 +617,34 @@ TEST_F(CollisionPreventionTest, jerkLimit)
 	EXPECT_GT(modified_setpoint_limited_jerk.norm(), modified_setpoint_default_jerk.norm());
 
 }
+TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData)
+{
+	// GIVEN: a vehicle attitude and a distance sensor message
+	TestCollisionPrevention cp;
+	cp.getObstacleMap().increment = 10.f;
+	matrix::Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
+	distance_sensor_s distance_sensor {};
+	distance_sensor.min_distance = 0.2f;
+	distance_sensor.max_distance = 20.f;
+	distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
+	// Distance is out of Range
+	distance_sensor.current_distance = -1.f;
+	distance_sensor.signal_quality = 0;
+
+	uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
+
+	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
+
+	//THEN: the correct bins in the map should be filled
+	for (uint32_t i = 0; i < distances_array_size; i++) {
+		if (i == 0) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], distance_sensor.max_distance * 100.f);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+	}
+}
 
 TEST_F(CollisionPreventionTest, addDistanceSensorData)
 {