From 67d0f5c5d169f25fd87bc61e75192e3582699dd2 Mon Sep 17 00:00:00 2001
From: baumanta <tanja@auterion.com>
Date: Wed, 22 Jan 2020 14:38:18 +0100
Subject: [PATCH] CollisionPrevention: No movement if FOV is zero

---
 .../CollisionPrevention.cpp                   | 16 +++++++++
 .../CollisionPreventionTest.cpp               | 34 ++++++++++++++++++-
 2 files changed, 49 insertions(+), 1 deletion(-)

diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp
index 56af69b804..8241800c6c 100644
--- a/src/lib/collision_prevention/CollisionPrevention.cpp
+++ b/src/lib/collision_prevention/CollisionPrevention.cpp
@@ -397,6 +397,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 	const float setpoint_length = setpoint.norm();
 
 	const hrt_abstime constrain_time = getTime();
+	int num_fov_bins = 0;
 
 	if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
 		if (setpoint_length > 0.001f) {
@@ -433,6 +434,11 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 				// get direction of current bin
 				const Vector2f bin_direction = {cosf(angle), sinf(angle)};
 
+				//count number of bins in the field of valid_new
+				if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) {
+					num_fov_bins ++;
+				}
+
 				if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
 				    && _obstacle_map_body_frame.distances[i] < UINT16_MAX) {
 
@@ -467,6 +473,16 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 				}
 			}
 
+			//if the sensor field of view is zero, never allow to move (even if move_no_data=1)
+			if (num_fov_bins == 0) {
+				vel_max = 0.f;
+
+				if (getElapsedTime(&_last_timeout_warning) > 1_s) {
+					mavlink_log_critical(&_mavlink_log_pub, "Range sensor data invalid, no movement allowed.");
+					_last_timeout_warning = getTime();
+				}
+			}
+
 			setpoint = setpoint_dir * vel_max;
 		}
 
diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp
index fd68b0ea0d..774a9cd2a6 100644
--- a/src/lib/collision_prevention/CollisionPreventionTest.cpp
+++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp
@@ -505,12 +505,34 @@ TEST_F(CollisionPreventionTest, goNoData)
 	matrix::Vector2f curr_pos(0, 0);
 	matrix::Vector2f curr_vel(2, 0);
 
+	// AND: an obstacle message
+	obstacle_distance_s message;
+	memset(&message, 0xDEAD, sizeof(message));
+	message.frame = message.MAV_FRAME_GLOBAL; //north aligned
+	message.min_distance = 100;
+	message.max_distance = 2000;
+	int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
+	message.increment = 360.f / distances_array_size;
+
+	//fov from 0deg to 20deg
+	for (int i = 0; i < distances_array_size; i++) {
+		float angle = i * message.increment;
+
+		if (angle > 0.f && angle < 40.f) {
+			message.distances[i] = 700;
+
+		} else {
+			message.distances[i] = UINT16_MAX;
+		}
+	}
+
 	// AND: a parameter handle
 	param_t param = param_handle(px4::params::CP_DIST);
 	float value = 5; // try to keep 5m distance
 	param_set(param, &value);
 	cp.paramsChanged();
 
+	// AND: a setpoint outside the field of view
 	matrix::Vector2f original_setpoint = {-5, 0};
 	matrix::Vector2f modified_setpoint = original_setpoint;
 
@@ -524,10 +546,20 @@ TEST_F(CollisionPreventionTest, goNoData)
 	param_set(param_allow, &value_allow);
 	cp.paramsChanged();
 
-	//THEN: the modified setpoint should stay the same as the input
+	//THEN: When all bins contain UINT_16MAX the setpoint should be zero even if CP_GO_NO_DATA=1
+	modified_setpoint = original_setpoint;
+	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
+	EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f);
+
+	//THEN: As soon as the range data contains any valid number, flying outside the FOV is allowed
+	message.timestamp = hrt_absolute_time();
+	orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
+	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
+
 	modified_setpoint = original_setpoint;
 	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
 	EXPECT_FLOAT_EQ(modified_setpoint.norm(), original_setpoint.norm());
+	orb_unadvertise(obstacle_distance_pub);
 }
 
 TEST_F(CollisionPreventionTest, jerkLimit)