added handling of -1 as out of range indicator for 1D distance sensors

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)) {
 

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│      │          //

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)
 {