Guidance feature for Collision Prevention (#13017)

* add guidance

* remove COL_PREV_ANG and replace with COL_PREV_CNG

* safe max ranges per bin

* increase default value for colprev delay to account for tracking delay

* update parameter description

* fix and extend testing

* add handling for overlapping sensor data

* fix decision process for overlapping sensors

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -79,6 +79,7 @@ CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
 
 	for (uint32_t i = 0 ; i < internal_bins; i++) {
 		_data_timestamps[i] = current_time;
+		_data_maxranges[i] = 0;
 		_obstacle_map_body_frame.distances[i] = UINT16_MAX;
 	}
 }
@@ -118,8 +119,11 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 
 			//add all data points inside to FOV
 			if (obstacle.distances[msg_index] != UINT16_MAX) {
-				_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
-				_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
+				if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
+					_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
+					_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
+					_data_maxranges[i] = obstacle.max_distance;
+				}
 			}
 
 		}
@@ -134,8 +138,12 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 
 			//add all data points inside to FOV
 			if (obstacle.distances[msg_index] != UINT16_MAX) {
-				_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
-				_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
+
+				if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
+					_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
+					_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
+					_data_maxranges[i] = obstacle.max_distance;
+				}
 			}
 
 		}
@@ -146,6 +154,37 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 	}
 }
 
+
+bool CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_reading)
+{
+	//use data from this sensor if:
+	//1. this sensor data is in range, the bin contains already valid data and this data is coming from the same or less range sensor
+	//2. this sensor data is in range, and the last reading was out of range
+	//3. this sensor data is out of range, the last reading was as well and this is the sensor with longest range
+	//4. this sensor data is out of range, the last reading was valid and coming from the same sensor
+
+	uint16_t sensor_range_cm = (int)(100 * sensor_range); //convert to cm
+
+	if (sensor_reading < sensor_range) {
+		if ((_obstacle_map_body_frame.distances[map_index] < _data_maxranges[map_index]
+		     && sensor_range_cm <= _data_maxranges[map_index])
+		    || _obstacle_map_body_frame.distances[map_index] >= _data_maxranges[map_index]) {
+			return true;
+
+		}
+
+	} else {
+		if ((_obstacle_map_body_frame.distances[map_index] >= _data_maxranges[map_index]
+		     && sensor_range_cm >= _data_maxranges[map_index])
+		    || (_obstacle_map_body_frame.distances[map_index] < _data_maxranges[map_index]
+			&& sensor_range_cm == _data_maxranges[map_index])) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 void CollisionPrevention::_updateObstacleMap()
 {
 	_sub_vehicle_attitude.update();
@@ -223,14 +262,64 @@ void CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sen
 		attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
 		float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
 
+		if (distance_reading < distance_sensor.max_distance) {
+			distance_reading = distance_reading * sensor_dist_scale;
+		}
+
+		uint16_t sensor_range = (int)(100 * distance_sensor.max_distance); //convert to cm
+
 		for (int bin = lower_bound; bin <= upper_bound; ++bin) {
 			int wrapped_bin = wrap_bin(bin);
 
-			// compensate measurement for vehicle tilt and convert to cm
-			_obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading * sensor_dist_scale);
-			_data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp;
+			if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
+				_obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading);
+				_data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp;
+				_data_maxranges[wrapped_bin] = sensor_range;
+			}
+		}
+	}
+}
+
+void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index,
+		float vehicle_yaw_angle_rad)
+{
+	float col_prev_d = _param_mpc_col_prev_d.get();
+	int guidance_bins = floor(_param_mpc_col_prev_cng.get() / INTERNAL_MAP_INCREMENT_DEG);
+	int sp_index_original = setpoint_index;
+	float best_cost = 9999.f;
+
+	for (int i = sp_index_original - guidance_bins; i <= sp_index_original + guidance_bins; i++) {
+
+		//apply moving average filter to the distance array to be able to center in larger gaps
+		int filter_size = 1;
+		float mean_dist = 0;
+
+		for (int j =  i - filter_size; j <= i + filter_size; j++) {
+			int bin = wrap_bin(j);
+
+			if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
+				mean_dist += col_prev_d * 100.f;
+
+			} else {
+				mean_dist += _obstacle_map_body_frame.distances[bin];
+			}
+		}
+
+		int bin = wrap_bin(i);
+		mean_dist = mean_dist / (2.f * filter_size + 1.f);
+		float deviation_cost = col_prev_d * 50.f * std::abs(i - sp_index_original);
+		float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
+
+		if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) {
+			best_cost = bin_cost;
+
+			float angle = math::radians((float)bin * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
+			angle  = wrap_2pi(vehicle_yaw_angle_rad + angle);
+			setpoint_dir = {cosf(angle), sinf(angle)};
+			setpoint_index = bin;
 		}
 	}
+
 }
 
 
@@ -242,7 +331,6 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 	//read parameters
 	float col_prev_d = _param_mpc_col_prev_d.get();
 	float col_prev_dly = _param_mpc_col_prev_dly.get();
-	float col_prev_ang_rad = math::radians(_param_mpc_col_prev_ang.get());
 	float xy_p = _param_mpc_xy_p.get();
 	float max_jerk = _param_mpc_jerk_max.get();
 	float max_accel = _param_mpc_acc_hor.get();
@@ -264,6 +352,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 			float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
 			int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
 
+			//change setpoint direction slightly (max by _param_mpc_col_prev_cng degrees) to help guide through narrow gaps
+			_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
+
+			//limit speed for safe flight
 			for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { //disregard unused bins at the end of the message
 
 				//delete stale values
@@ -274,6 +366,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 				}
 
 				float distance = _obstacle_map_body_frame.distances[i] * 0.01f; //convert to meters
+				float max_range = _data_maxranges[i] * 0.01f; //convert to meters
 				float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
 
 				// convert from body to local frame in the range [0, 2*pi]
@@ -285,16 +378,25 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 				if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
 				    && _obstacle_map_body_frame.distances[i] < UINT16_MAX) {
 
-					if (setpoint_dir.dot(bin_direction) > 0
-					    && setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
+					if (setpoint_dir.dot(bin_direction) > 0) {
 						//calculate max allowed velocity with a P-controller (same gain as in the position controller)
 						float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
-						float delay_distance = curr_vel_parallel * (col_prev_dly + data_age * 1e-6f);
+						float delay_distance = curr_vel_parallel * col_prev_dly;
+
+						if (distance < max_range) {
+							delay_distance += curr_vel_parallel * (data_age * 1e-6f);
+						}
+
 						float stop_distance =  math::max(0.f, distance - min_dist_to_keep - delay_distance);
 						float vel_max_posctrl = xy_p * stop_distance;
+
 						float vel_max_smooth = math::trajectory::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
-						Vector2f  vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth);
-						float vel_max_bin = vel_max_vec.dot(setpoint_dir);
+						float projection = bin_direction.dot(setpoint_dir);
+						float vel_max_bin = vel_max;
+
+						if (projection > 0.01f) {
+							vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection;
+						}
 
 						//constrain the velocity
 						if (vel_max_bin >= 0) {
@@ -311,7 +413,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 		}
 
 	} else {
-		// if distance data are stale, switch to Loiter
+		// if distance data is stale, switch to Loiter
 		_publishVehicleCmdDoLoiter();
 		mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
 	}

src/lib/CollisionPrevention/CollisionPrevention.hpp

@@ -84,8 +84,10 @@ protected:
 
 	obstacle_distance_s _obstacle_map_body_frame {};
 	uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
+	uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof(
+										    _obstacle_map_body_frame.distances[0])]; /**< in cm */
 
-	void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude);
+	void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude);
 
 	/**
 	 * Updates obstacle distance message with measurement from offboard
@@ -93,6 +95,24 @@ protected:
 	 */
 	void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
 
+	/**
+	 * Computes an adaption to the setpoint direction to guide towards free space
+	 * @param setpoint_dir, setpoint direction before collision prevention intervention
+	 * @param setpoint_index, index of the setpoint in the internal obstacle map
+	 * @param vehicle_yaw_angle_rad, vehicle orientation
+	 */
+	void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);
+
+	/**
+	 * Determines whether a new sensor measurement is used
+	 * @param map_index, index of the bin in the internal map the measurement belongs in
+	 * @param sensor_range, max range of the sensor in meters
+	 * @param sensor_reading, distance measurement in meters
+	 */
+	bool _enterData(int map_index, float sensor_range, float sensor_reading);
+
+
+	//Timing functions. Necessary to mock time in the tests
 	virtual hrt_abstime getTime();
 	virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr);
 
@@ -115,7 +135,7 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */
-		(ParamFloat<px4::params::MPC_COL_PREV_ANG>) _param_mpc_col_prev_ang, /**< collision prevention detection angle */
+		(ParamFloat<px4::params::MPC_COL_PREV_CNG>) _param_mpc_col_prev_cng, /**< collision prevention change setpoint angle */
 		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
 		(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly, /**< delay of the range measurement data*/
 		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
@@ -153,8 +173,6 @@ private:
 		return offset;
 	}
 
-
-
 	/**
 	 * Computes collision free setpoints
 	 * @param setpoint, setpoint before collision prevention intervention

src/lib/CollisionPrevention/CollisionPreventionTest.cpp

@@ -60,6 +60,15 @@ public:
 	{
 		_addObstacleSensorData(obstacle, attitude);
 	}
+	void test_adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index,
+					 float vehicle_yaw_angle_rad)
+	{
+		_adaptSetpointDirection(setpoint_dir, setpoint_index, vehicle_yaw_angle_rad);
+	}
+	bool test_enterData(int map_index, float sensor_range, float sensor_reading)
+	{
+		return _enterData(map_index, sensor_range, sensor_reading);
+	}
 };
 
 class TestTimingCollisionPrevention : public TestCollisionPrevention
@@ -176,7 +185,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
 	EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
 
 	EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1);
-	EXPECT_EQ(original_setpoint2, modified_setpoint2);
+	EXPECT_FLOAT_EQ(original_setpoint2.norm(), modified_setpoint2.norm());
 }
 
 TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
@@ -303,7 +312,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
 
 		if (i < 6) {
 			// THEN: If the data is new enough, the velocity setpoint should stay the same as the input
-			EXPECT_EQ(original_setpoint, modified_setpoint);
+			// Note: direction will change slightly due to guidance
+			EXPECT_EQ(original_setpoint.norm(), modified_setpoint.norm());
 
 		} else {
 			// THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data
@@ -404,16 +414,16 @@ TEST_F(CollisionPreventionTest, outsideFOV)
 		orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
 		cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
 
-		if (angle_deg > 50.f && angle_deg < 230.f) {
-			// THEN: inside the FOV the setpoint should be limited
-			EXPECT_GT(modified_setpoint.norm(), 0.f);
-			EXPECT_LT(modified_setpoint.norm(), 10.f);
+		//THEN: if the resulting setpoint demands velocities bigger zero, it must lie inside the FOV
+		float setpoint_length = modified_setpoint.norm();
 
-		} else {
-			// THEN: outside the FOV the setpoint should be clamped to zero
-			EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f);
+		if (setpoint_length > 0.f) {
+			matrix::Vector2f setpoint_dir = modified_setpoint / setpoint_length;
+			float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0));
+			float sp_angle_deg = math::degrees(matrix::wrap_2pi(sp_angle_body_frame));
+			EXPECT_GT(sp_angle_deg, 45.f);
+			EXPECT_LT(sp_angle_deg, 225.f);
 		}
-
 	}
 
 	orb_unadvertise(obstacle_distance_pub);
@@ -804,3 +814,249 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset)
 		cp.getObstacleMap().distances[i] = UINT16_MAX;
 	}
 }
+
+TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
+{
+	// GIVEN: a vehicle attitude and obstacle distance message
+	TestCollisionPrevention cp;
+	cp.getObstacleMap().increment = 10.f;
+	obstacle_distance_s obstacle_msg {};
+	obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
+	obstacle_msg.increment = 10.f;
+	obstacle_msg.min_distance = 20;
+	obstacle_msg.max_distance = 2000;
+	obstacle_msg.angle_offset = 0.f;
+
+	matrix::Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
+	float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi();
+
+	//obstacle at 0-30 deg world frame, distance 5 meters
+	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
+
+	for (int i = 0; i < 7 ; i++) {
+		obstacle_msg.distances[i] = 500;
+	}
+
+	obstacle_msg.distances[2] = 1000;
+
+	//define setpoint
+	matrix::Vector2f setpoint_dir(1, 0);
+	float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
+	float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset,
+					 0.f, 360.f);
+	int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment);
+
+	//set parameter
+	param_t param = param_handle(px4::params::MPC_COL_PREV_D);
+	float value = 3; // try to keep 10m away from obstacles
+	param_set(param, &value);
+	cp.paramsChanged();
+
+	//WHEN: we add distance sensor data
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude);
+	cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
+
+	//THEN: the setpoint direction should be modified correctly
+	EXPECT_EQ(sp_index, 2);
+	EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262);
+	EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012);
+}
+
+TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
+{
+	// GIVEN: a vehicle attitude and obstacle distance message
+	TestCollisionPrevention cp;
+	cp.getObstacleMap().increment = 10.f;
+	obstacle_distance_s obstacle_msg {};
+	obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
+	obstacle_msg.increment = 10.f;
+	obstacle_msg.min_distance = 20;
+	obstacle_msg.max_distance = 2000;
+	obstacle_msg.angle_offset = 0.f;
+
+	matrix::Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
+	float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi();
+
+	//obstacle at 0-30 deg world frame, distance 5 meters
+	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
+
+	for (int i = 0; i < 7 ; i++) {
+		obstacle_msg.distances[i] = 500;
+	}
+
+	obstacle_msg.distances[1] = 1000;
+	obstacle_msg.distances[2] = 1000;
+	obstacle_msg.distances[3] = 1000;
+
+	//define setpoint
+	matrix::Vector2f setpoint_dir(1, 0);
+	float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
+	float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset,
+					 0.f, 360.f);
+	int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment);
+
+	//set parameter
+	param_t param = param_handle(px4::params::MPC_COL_PREV_D);
+	float value = 3; // try to keep 10m away from obstacles
+	param_set(param, &value);
+	cp.paramsChanged();
+
+	//WHEN: we add distance sensor data
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude);
+	cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
+
+	//THEN: the setpoint direction should be modified correctly
+	EXPECT_EQ(sp_index, 2);
+	EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262);
+	EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012);
+}
+
+TEST_F(CollisionPreventionTest, overlappingSensors)
+{
+	// GIVEN: a simple setup condition
+	TestCollisionPrevention cp;
+	matrix::Vector2f original_setpoint(10, 0);
+	float max_speed = 3;
+	matrix::Vector2f curr_pos(0, 0);
+	matrix::Vector2f curr_vel(2, 0);
+	vehicle_attitude_s attitude;
+	attitude.timestamp = hrt_absolute_time();
+	attitude.q[0] = 1.0f;
+	attitude.q[1] = 0.0f;
+	attitude.q[2] = 0.0f;
+	attitude.q[3] = 0.0f;
+
+	// AND: a parameter handle
+	param_t param = param_handle(px4::params::MPC_COL_PREV_D);
+	float value = 10; // try to keep 10m distance
+	param_set(param, &value);
+	cp.paramsChanged();
+
+	// AND: an obstacle message for a short range and a long range sensor
+	obstacle_distance_s short_range_msg, short_range_msg_no_obstacle, long_range_msg;
+	memset(&short_range_msg, 0xDEAD, sizeof(short_range_msg));
+	short_range_msg.frame = short_range_msg.MAV_FRAME_GLOBAL; //north aligned
+	short_range_msg.angle_offset = 0;
+	short_range_msg.timestamp = hrt_absolute_time();
+	int distances_array_size = sizeof(short_range_msg.distances) / sizeof(short_range_msg.distances[0]);
+	short_range_msg.increment = 360 / distances_array_size;
+	long_range_msg = short_range_msg;
+	long_range_msg.min_distance = 100;
+	long_range_msg.max_distance = 1000;
+	short_range_msg.min_distance = 20;
+	short_range_msg.max_distance = 200;
+	short_range_msg_no_obstacle = short_range_msg;
+
+
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i < 10) {
+			short_range_msg_no_obstacle.distances[i] = 201;
+			short_range_msg.distances[i] = 150;
+			long_range_msg.distances[i] = 500;
+
+		} else {
+			short_range_msg_no_obstacle.distances[i] = UINT16_MAX;
+			short_range_msg.distances[i] = UINT16_MAX;
+			long_range_msg.distances[i] = UINT16_MAX;
+		}
+	}
+
+
+	// CASE 1
+	//WHEN: we publish the long range sensor message
+	orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &long_range_msg);
+	orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
+	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg);
+	orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
+	matrix::Vector2f modified_setpoint = original_setpoint;
+	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
+
+	// THEN: the internal map data should contain the long range measurement
+	EXPECT_EQ(500, cp.getObstacleMap().distances[2]);
+
+	// CASE 2
+	// WHEN: we publish the short range message followed by a long range message
+	short_range_msg.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg);
+	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
+	long_range_msg.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg);
+	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
+
+	// THEN: the internal map data should contain the short range measurement
+	EXPECT_EQ(150, cp.getObstacleMap().distances[2]);
+
+	// CASE 3
+	// WHEN: we publish the short range message with values out of range followed by a long range message
+	short_range_msg_no_obstacle.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg_no_obstacle);
+	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
+	long_range_msg.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg);
+	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
+
+	// THEN: the internal map data should contain the short range measurement
+	EXPECT_EQ(500, cp.getObstacleMap().distances[2]);
+
+	orb_unadvertise(obstacle_distance_pub);
+	orb_unadvertise(vehicle_attitude_pub);
+}
+
+TEST_F(CollisionPreventionTest, enterData)
+{
+	// GIVEN: a simple setup condition
+	TestCollisionPrevention cp;
+	cp.getObstacleMap().increment = 10.f;
+	matrix::Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
+
+	//THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted
+	EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
+	EXPECT_TRUE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
+	EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
+	EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
+
+	//WHEN: we add distance sensor data to the right with a valid reading
+	distance_sensor_s distance_sensor {};
+	distance_sensor.min_distance = 0.2f;
+	distance_sensor.max_distance = 20.f;
+	distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING;
+	distance_sensor.h_fov = math::radians(19.99f);
+	distance_sensor.current_distance = 5.f;
+	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
+
+	//THEN: the internal map should contain the distance sensor readings
+	EXPECT_EQ(500, cp.getObstacleMap().distances[8]);
+	EXPECT_EQ(500, cp.getObstacleMap().distances[9]);
+
+	//THEN: bins 8 & 9 contain valid readings
+	// a valid reading should only be accepted from sensors with shorter or equal range
+	// a out of range reading should only be accepted from sensors with the same range
+
+	EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
+	EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
+	EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
+	EXPECT_FALSE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
+	EXPECT_FALSE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
+
+	//WHEN: we add distance sensor data to the right with an out of range reading
+	distance_sensor.current_distance = 21.f;
+	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
+
+	//THEN: the internal map should contain the distance sensor readings
+	EXPECT_EQ(2000, cp.getObstacleMap().distances[8]);
+	EXPECT_EQ(2000, cp.getObstacleMap().distances[9]);
+
+	//THEN: bins 8 & 9 contain readings out of range
+	// a reading in range will be accepted in any case
+	// out of range readings will only be accepted from sensors with bigger or equal range
+
+	EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
+	EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
+	EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
+	EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
+}

src/lib/CollisionPrevention/collisionprevention_params.c

@@ -42,7 +42,7 @@
 /**
  * Minimum distance the vehicle should keep to all obstacles
  *
- * Only used in Position mode. Collision avoidace is disabled by setting this parameter to a negative value
+ * Only used in Position mode. Collision avoidance is disabled by setting this parameter to a negative value
  *
  * @min -1
  * @max 15
@@ -52,7 +52,7 @@
 PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
 
 /**
- * Average delay of the range sensor message in seconds
+ * Average delay of the range sensor message plus the tracking delay of the position controller in seconds
  *
  * Only used in Position mode.
  *
@@ -61,10 +61,10 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
  * @unit seconds
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f);
+PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.4f);
 
 /**
- * Angle left/right from the commanded setpoint in which the range data is used to calculate speed limitations. All data further from the commanded direction is not considered.
+ * Angle left/right from the commanded setpoint by which the collision prevention algorithm can choose to change the setpoint direction
  *
  * Only used in Position mode.
  *
@@ -73,4 +73,4 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f);
  * @unit [deg]
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_COL_PREV_ANG, 45.f);
+PARAM_DEFINE_FLOAT(MPC_COL_PREV_CNG, 30.f);