Collision Prevention: support multiple sensors and frames (#12883)

* build internal sensor map

* Extend testing coverage

* Update matrix library

cmake/px4_add_common_flags.cmake

@@ -51,6 +51,7 @@ function(px4_add_common_flags)
 		-fdata-sections
 		-ffunction-sections
 		-fomit-frame-pointer
+		-fmerge-all-constants
 
 		#-funsafe-math-optimizations # Enables -fno-signed-zeros, -fno-trapping-math, -fassociative-math and -freciprocal-math
 		-fno-signed-zeros	# Allow optimizations for floating-point arithmetic that ignore the signedness of zero

msg/obstacle_distance.msg

@@ -1,6 +1,11 @@
 # Obstacle distances in front of the sensor.
 uint64 timestamp		# time since system start (microseconds)
 
+uint8 frame		#Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is North aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.
+uint8 MAV_FRAME_GLOBAL = 0
+uint8 MAV_FRAME_LOCAL_NED = 1
+uint8 MAV_FRAME_BODY_FRD = 12
+
 uint8 sensor_type # Type from MAV_DISTANCE_SENSOR enum.
 uint8 MAV_DISTANCE_SENSOR_LASER = 0
 uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -43,11 +43,46 @@
 
 using namespace matrix;
 using namespace time_literals;
+namespace
+{
+static const int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly
+static const int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG;
 
+float wrap_360(float f)
+{
+	return wrap(f, 0.f, 360.f);
+}
+
+int wrap_bin(int i)
+{
+	i = i % INTERNAL_MAP_USED_BINS;
+
+	while (i < 0) {
+		i += INTERNAL_MAP_USED_BINS;
+	}
+
+	return i;
+}
+}
 
 CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
 	ModuleParams(parent)
 {
+	static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5");
+	static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly");
+	//initialize internal obstacle map
+	_obstacle_map_body_frame.timestamp = getTime();
+	_obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG;
+	_obstacle_map_body_frame.min_distance = UINT16_MAX;
+	_obstacle_map_body_frame.max_distance = 0;
+	_obstacle_map_body_frame.angle_offset = 0.f;
+	uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]);
+	uint64_t current_time = getTime();
+
+	for (uint32_t i = 0 ; i < internal_bins; i++) {
+		_data_timestamps[i] = current_time;
+		_obstacle_map_body_frame.distances[i] = UINT16_MAX;
+	}
 }
 
 CollisionPrevention::~CollisionPrevention()
@@ -58,143 +93,195 @@ CollisionPrevention::~CollisionPrevention()
 	}
 }
 
-void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
+hrt_abstime CollisionPrevention::getTime()
 {
-	_sub_obstacle_distance.update();
+	return hrt_absolute_time();
-	const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
+}
 
-	// Update with offboard data if the data is not stale
+hrt_abstime CollisionPrevention::getElapsedTime(const hrt_abstime *ptr)
-	if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) {
+{
-		obstacle = obstacle_distance;
+	return hrt_absolute_time() - *ptr;
+}
+
+void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle,
+		const matrix::Quatf &vehicle_attitude)
+{
+	int msg_index = 0;
+	float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi());
+	float increment_factor = 1.f / obstacle.increment;
+
+
+	if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) {
+		//Obstacle message arrives in local_origin frame (north aligned)
+		//corresponding data index (convert to world frame and shift by msg offset)
+		for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
+			float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset;
+			msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor);
+
+			//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;
+			}
+
+		}
+
+	} else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) {
+		//Obstacle message arrives in body frame (front aligned)
+		//corresponding data index (shift by msg offset)
+		for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
+			float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG +
+					      _obstacle_map_body_frame.angle_offset;
+			msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor);
+
+			//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;
+			}
+
+		}
+
+	} else {
+		mavlink_log_critical(&_mavlink_log_pub, "Obstacle message received in unsupported frame %.0f\n",
+				     (double)obstacle.frame);
 	}
 }
 
-void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
+void CollisionPrevention::_updateObstacleMap()
 {
+	_sub_vehicle_attitude.update();
+
+	// add distance sensor data
 	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+
+		//if a new distance sensor message has arrived
+		if (_sub_distance_sensor[i].updated()) {
 			distance_sensor_s distance_sensor {};
 			_sub_distance_sensor[i].copy(&distance_sensor);
 
-		// consider only instaces with updated, valid data and orientations useful for collision prevention
+			// consider only instances with valid data and orientations useful for collision prevention
-		if ((hrt_elapsed_time(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
+			if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
 			    (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
 			    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
 
-
+				//update message description
-			if (obstacle.increment > 0) {
+				_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
-				// data from companion
+				_obstacle_map_body_frame.max_distance = math::max((int)_obstacle_map_body_frame.max_distance,
-				obstacle.timestamp = math::max(obstacle.timestamp, distance_sensor.timestamp);
-				obstacle.max_distance = math::max((int)obstacle.max_distance,
 									(int)distance_sensor.max_distance * 100);
-				obstacle.min_distance = math::min((int)obstacle.min_distance,
+				_obstacle_map_body_frame.min_distance = math::min((int)_obstacle_map_body_frame.min_distance,
 									(int)distance_sensor.min_distance * 100);
-				// since the data from the companion are already in the distances data structure,
-				// keep the increment that is sent
-				obstacle.angle_offset = 0.f; //companion not sending this field (needs mavros update)
 
-			} else {
+				_addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q));
-				obstacle.timestamp = distance_sensor.timestamp;
+			}
-				obstacle.max_distance = distance_sensor.max_distance * 100; // convert to cm
+		}
-				obstacle.min_distance = distance_sensor.min_distance * 100; // convert to cm
-				memset(&obstacle.distances[0], 0xff, sizeof(obstacle.distances));
-				obstacle.increment = math::degrees(distance_sensor.h_fov);
-				obstacle.angle_offset = 0.f;
 	}
 
-			if ((distance_sensor.current_distance > distance_sensor.min_distance) &&
+	// add obstacle distance data
-			    (distance_sensor.current_distance < distance_sensor.max_distance)) {
+	if (_sub_obstacle_distance.update()) {
+		const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
 
-				float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset);
+		// Update map with obstacle data if the data is not stale
+		if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
+			//update message description
+			_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp);
+			_obstacle_map_body_frame.max_distance = math::max((int)_obstacle_map_body_frame.max_distance,
+								(int)obstacle_distance.max_distance);
+			_obstacle_map_body_frame.min_distance = math::min((int)_obstacle_map_body_frame.min_distance,
+								(int)obstacle_distance.min_distance);
+			_addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q));
+		}
+	}
 
-				matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
+	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
-				// convert the sensor orientation from body to local frame in the range [0, 360]
+	_obstacle_distance_pub.publish(_obstacle_map_body_frame);
-				float sensor_yaw_local_deg  = math::degrees(wrap_2pi(Eulerf(attitude).psi() + sensor_yaw_body_rad));
+}
+
+void CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
+		const matrix::Quatf &vehicle_attitude)
+{
+	//clamp at maximum sensor range
+	float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance);
+
+	//discard values below min range
+	if ((distance_reading > distance_sensor.min_distance)) {
+
+		float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset);
+		float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
 
 		// calculate the field of view boundary bin indices
-				int lower_bound = (int)floor((sensor_yaw_local_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) /
+		int lower_bound = (int)floor((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) /
-							     obstacle.increment);
+					     INTERNAL_MAP_INCREMENT_DEG);
-				int upper_bound = (int)floor((sensor_yaw_local_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) /
+		int upper_bound = (int)floor((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) /
-							     obstacle.increment);
+					     INTERNAL_MAP_INCREMENT_DEG);
-
+
-				// if increment is lower than 5deg, use an offset
+		//floor values above zero, ceil values below zero
-				const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);
+		if (lower_bound < 0) { lower_bound++; }
-
+
-				if (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size
+		if (upper_bound < 0) { upper_bound++; }
-						|| upper_bound >= distances_array_size)) && obstacle.increment < 5.f) {
-					obstacle.angle_offset = sensor_yaw_local_deg ;
-					upper_bound  = abs(upper_bound - lower_bound);
-					lower_bound  = 0;
-				}
 
 		// rotate vehicle attitude into the sensor body frame
-				matrix::Quatf attitude_sensor_frame = attitude;
+		matrix::Quatf attitude_sensor_frame = vehicle_attitude;
 		attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
-				float attitude_sensor_frame_pitch = cosf(Eulerf(attitude_sensor_frame).theta());
+		float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
 
 		for (int bin = lower_bound; bin <= upper_bound; ++bin) {
-					int wrap_bin = bin;
+			int wrapped_bin = wrap_bin(bin);
-
-					if (wrap_bin < 0) {
-						// wrap bin index around the array
-						wrap_bin = (int)floor(360.f / obstacle.increment) + bin;
-					}
-
-					if (wrap_bin >= distances_array_size) {
-						// wrap bin index around the array
-						wrap_bin = bin - distances_array_size;
-					}
 
 			// compensate measurement for vehicle tilt and convert to cm
-					obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin],
+			_obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading * sensor_dist_scale);
-								       (int)(100 * distance_sensor.current_distance * attitude_sensor_frame_pitch));
+			_data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp;
 		}
 	}
-		}
-	}
-
-	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
-	_obstacle_distance_pub.publish(obstacle);
 }
 
+
 void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 		const Vector2f &curr_pos, const Vector2f &curr_vel)
 {
-	obstacle_distance_s obstacle{};
+	_updateObstacleMap();
-	_updateOffboardObstacleDistance(obstacle);
-	_updateDistanceSensor(obstacle);
 
-	float setpoint_length = setpoint.norm();
+	//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();
+	matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
+	float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
+
+	float setpoint_length = setpoint.norm();
 
-	if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
+	if (getElapsedTime(&_obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
 		if (setpoint_length > 0.001f) {
 
 			Vector2f setpoint_dir = setpoint / setpoint_length;
 			float vel_max = setpoint_length;
-			int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);
+			float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);
-			float min_dist_to_keep = math::max(obstacle.min_distance / 100.0f, col_prev_d);
-
-			for (int i = 0; i < distances_array_size; i++) {
 
-				if ((float)i * obstacle.increment < 360.f) { //disregard unused bins at the end of the message
+			float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
+			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);
 
-					float distance = obstacle.distances[i] / 100.0f; //convert to meters
+			for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { //disregard unused bins at the end of the message
-					float angle = math::radians((float)i * obstacle.increment);
 
-					if (obstacle.angle_offset > 0.f) {
+				//delete stale values
-						angle += math::radians(obstacle.angle_offset);
+				if (getElapsedTime(&_data_timestamps[i]) > RANGE_STREAM_TIMEOUT_US) {
+					_obstacle_map_body_frame.distances[i] = UINT16_MAX;
 				}
 
+				float distance = _obstacle_map_body_frame.distances[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]
+				angle  = wrap_2pi(vehicle_yaw_angle_rad + angle);
+
 				//get direction of current bin
 				Vector2f bin_direction = {cos(angle), sin(angle)};
 
-					if (obstacle.distances[i] < obstacle.max_distance &&
+				if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
-					    obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
+				    && _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)) {
@@ -213,17 +300,9 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 						}
 					}
 
-					} else if (obstacle.distances[i] == UINT16_MAX) {
+				} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
-						float sp_bin = setpoint_dir.dot(bin_direction);
-						float ang_half_bin = cosf(math::radians(obstacle.increment) / 2.f);
-
-						//if the setpoint lies outside the FOV set velocity to zero
-						if (sp_bin > ang_half_bin) {
 					vel_max = 0.f;
 				}
-
-					}
-				}
 			}
 
 			setpoint = setpoint_dir * vel_max;

src/lib/CollisionPrevention/CollisionPrevention.hpp

@@ -64,7 +64,7 @@ class CollisionPrevention : public ModuleParams
 public:
 	CollisionPrevention(ModuleParams *parent);
 
-	~CollisionPrevention();
+	virtual ~CollisionPrevention();
 	/**
 	 * Returs true if Collision Prevention is running
 	 */
@@ -80,6 +80,23 @@ public:
 	void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
 			    const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
 
+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])];
+
+	void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude);
+
+	/**
+	 * Updates obstacle distance message with measurement from offboard
+	 * @param obstacle, obstacle_distance message to be updated
+	 */
+	void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
+
+	virtual hrt_abstime getTime();
+	virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr);
+
+
 private:
 
 	bool _interfering{false};		/**< states if the collision prevention interferes with the user input */
@@ -136,6 +153,8 @@ private:
 		return offset;
 	}
 
+
+
 	/**
 	 * Computes collision free setpoints
 	 * @param setpoint, setpoint before collision prevention intervention
@@ -146,16 +165,22 @@ private:
 					   const matrix::Vector2f &curr_vel);
 
 	/**
-	 * Updates obstacle distance message with measurement from offboard
+	 * Publishes collision_constraints message
-	 * @param obstacle, obstacle_distance message to be updated
+	 * @param original_setpoint, setpoint before collision prevention intervention
+	 * @param adapted_setpoint, collision prevention adaped setpoint
 	 */
-	void _updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
+	void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
 
 	/**
-	 * Updates obstacle distance message with measurement from distance sensors
+	 * Publishes obstacle_distance message with fused data from offboard and from distance sensors
-	 * @param obstacle, obstacle_distance message to be updated
+	 * @param obstacle, obstacle_distance message to be publsihed
+	 */
+	void _publishObstacleDistance(obstacle_distance_s &obstacle);
+
+	/**
+	 * Aggregates the sensor data into a internal obstacle map in body frame
 	 */
-	void _updateDistanceSensor(obstacle_distance_s &obstacle);
+	void _updateObstacleMap();
 
 	/**
 	 * Publishes vehicle command.

src/lib/CollisionPrevention/CollisionPreventionTest.cpp

@@ -35,6 +35,7 @@
 #include <CollisionPrevention/CollisionPrevention.hpp>
 
 // to run: make tests TESTFILTER=CollisionPrevention
+hrt_abstime mocked_time = 0;
 
 class CollisionPreventionTest : public ::testing::Test
 {
@@ -50,6 +51,31 @@ class TestCollisionPrevention : public CollisionPrevention
 public:
 	TestCollisionPrevention() : CollisionPrevention(nullptr) {}
 	void paramsChanged() {CollisionPrevention::updateParamsImpl();}
+	obstacle_distance_s &getObstacleMap() {return _obstacle_map_body_frame;}
+	void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude)
+	{
+		_addDistanceSensorData(distance_sensor, attitude);
+	}
+	void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &attitude)
+	{
+		_addObstacleSensorData(obstacle, attitude);
+	}
+};
+
+class TestTimingCollisionPrevention : public TestCollisionPrevention
+{
+public:
+	TestTimingCollisionPrevention() : TestCollisionPrevention() {}
+protected:
+	hrt_abstime getTime() override
+	{
+		return mocked_time;
+	}
+
+	hrt_abstime getElapsedTime(const hrt_abstime *ptr) override
+	{
+		return mocked_time - *ptr;
+	}
 };
 
 TEST_F(CollisionPreventionTest, instantiation) { CollisionPrevention cp(nullptr); }
@@ -58,20 +84,12 @@ TEST_F(CollisionPreventionTest, behaviorOff)
 {
 	// GIVEN: a simple setup condition
 	TestCollisionPrevention cp;
-	matrix::Vector2f original_setpoint(10, 0);
-	float max_speed = 3.f;
-	matrix::Vector2f curr_pos(0, 0);
-	matrix::Vector2f curr_vel(2, 0);
-
-	// WHEN: we check if the setpoint should be modified
-	matrix::Vector2f modified_setpoint = original_setpoint;
-	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
 
-	// THEN: it should be the same
+	// THEN: the collision prevention should be turned off by default
-	EXPECT_EQ(original_setpoint, modified_setpoint);
+	EXPECT_FALSE(cp.is_active());
 }
 
-TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
+TEST_F(CollisionPreventionTest, noSensorData)
 {
 	// GIVEN: a simple setup condition
 	TestCollisionPrevention cp;
@@ -83,7 +101,6 @@ TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
 	// AND: a parameter handle
 	param_t param = param_handle(px4::params::MPC_COL_PREV_D);
 
-
 	// WHEN: we set the parameter check then apply the setpoint modification
 	float value = 10; // try to keep 10m away from obstacles
 	param_set(param, &value);
@@ -92,18 +109,26 @@ TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
 	matrix::Vector2f modified_setpoint = original_setpoint;
 	cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
 
-	// THEN: it shouldn't interfere with the setpoint, because there isn't an obstacle
+	// THEN: collision prevention should be enabled and limit the speed to zero
-	EXPECT_EQ(original_setpoint, modified_setpoint);
+	EXPECT_TRUE(cp.is_active());
+	EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm());
 }
 
-TEST_F(CollisionPreventionTest, testBehaviorOnWithAnObstacle)
+TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
 {
 	// GIVEN: a simple setup condition
 	TestCollisionPrevention cp;
-	matrix::Vector2f original_setpoint(10, 0);
+	matrix::Vector2f original_setpoint1(10, 0);
+	matrix::Vector2f original_setpoint2(-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);
@@ -114,25 +139,180 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithAnObstacle)
 	// 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 = 1000;
+	message.max_distance = 10000;
+	message.angle_offset = 0;
 	message.timestamp = hrt_absolute_time();
 	int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
 	message.increment = 360 / distances_array_size;
 
 	for (int i = 0; i < distances_array_size; i++) {
+		if (i < 10) {
 			message.distances[i] = 101;
+
+		} else {
+			message.distances[i] = 10001;
+		}
+
 	}
 
 	// WHEN: we publish the message and set the parameter and then run the setpoint modification
 	orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
+	orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
 	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
+	orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
+	matrix::Vector2f modified_setpoint1 = original_setpoint1;
+	matrix::Vector2f modified_setpoint2 = original_setpoint2;
+	cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel);
+	cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel);
+	orb_unadvertise(obstacle_distance_pub);
+	orb_unadvertise(vehicle_attitude_pub);
+
+	// THEN: the internal map should know the obstacle
+	// case 1: the velocity setpoint should be cut down to zero
+	// case 2: the velocity setpoint should stay the same as the input
+	EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
+	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);
+}
+
+TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
+{
+	// GIVEN: a simple setup condition
+	TestCollisionPrevention cp;
+	matrix::Vector2f original_setpoint1(10, 0);
+	matrix::Vector2f original_setpoint2(-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
+	distance_sensor_s message;
+	message.timestamp = hrt_absolute_time();
+	message.min_distance = 1.f;
+	message.max_distance = 100.f;
+	message.current_distance = 1.1f;
+
+	message.variance = 0.1f;
+	message.signal_quality = 100;
+	message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	message.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
+	message.h_fov = math::radians(50.f);
+	message.v_fov = math::radians(30.f);
+
+	// WHEN: we publish the message and set the parameter and then run the setpoint modification
+	orb_advert_t distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &message);
+	orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
+	orb_publish(ORB_ID(distance_sensor), distance_sensor_pub, &message);
+	orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
+
+	//WHEN:  We run the setpoint modification
+	matrix::Vector2f modified_setpoint1 = original_setpoint1;
+	matrix::Vector2f modified_setpoint2 = original_setpoint2;
+	cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel);
+	cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel);
+	orb_unadvertise(distance_sensor_pub);
+	orb_unadvertise(vehicle_attitude_pub);
+
+	// THEN: the internal map should know the obstacle
+	// case 1: the velocity setpoint should be cut down to zero because there is an obstacle
+	// case 2: the velocity setpoint should be cut down to zero because there is no data
+	EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
+	EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
+
+	EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1);
+	EXPECT_FLOAT_EQ(0.f, modified_setpoint2.norm()) << modified_setpoint2(0) << "," << modified_setpoint2(1);
+}
+
+TEST_F(CollisionPreventionTest, testPurgeOldData)
+{
+	// GIVEN: a simple setup condition
+	hrt_abstime start_time = hrt_absolute_time();
+	mocked_time = start_time;
+	TestTimingCollisionPrevention 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 = start_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
+	obstacle_distance_s message, message_empty;
+	memset(&message, 0xDEAD, sizeof(message));
+	message.frame = message.MAV_FRAME_GLOBAL; //north aligned
+	message.min_distance = 100;
+	message.max_distance = 10000;
+	message.angle_offset = 0;
+	message.timestamp = start_time;
+	int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
+	message.increment = 360 / distances_array_size;
+	message_empty = message;
+
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i < 10) {
+			message.distances[i] = 10001;
+
+		} else {
+			message.distances[i] = UINT16_MAX;
+		}
+
+		message_empty.distances[i] = UINT16_MAX;
+	}
+
+
+	// WHEN: we publish the message and set the parameter and then run the setpoint modification
+	orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
+	orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
+	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
+	orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
+
+	for (int i = 0; i < 10; i++) {
+
 		matrix::Vector2f modified_setpoint = original_setpoint;
 		cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
-	orb_unadvertise(obstacle_distance_pub);
 
-	// THEN: it should be cut down to zero
+		mocked_time = mocked_time + 100000; //advance time by 0.1 seconds
+		message_empty.timestamp = mocked_time;
+		orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message_empty);
+
+		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);
+
+		} else {
+			// THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data
 			EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1);
+		}
+	}
+
+	orb_unadvertise(obstacle_distance_pub);
+	orb_unadvertise(vehicle_attitude_pub);
 }
 
 TEST_F(CollisionPreventionTest, noBias)
@@ -156,6 +336,7 @@ TEST_F(CollisionPreventionTest, noBias)
 	message.min_distance = 100;
 	message.max_distance = 2000;
 	message.timestamp = hrt_absolute_time();
+	message.frame = message.MAV_FRAME_GLOBAL; //north aligned
 	int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
 	message.increment = 360 / distances_array_size;
 
@@ -192,14 +373,17 @@ TEST_F(CollisionPreventionTest, outsideFOV)
 	// 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 / distances_array_size;
+	message.increment = 360.f / distances_array_size;
 
-	//fov from i = 1/4 * distances_array_size to i = 3/4 * distances_array_size
+	//fov from 45deg to 225deg
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i > 0.25f * distances_array_size && i < 0.75f * distances_array_size) {
+		float angle = i * message.increment;
+
+		if (angle > 45.f && angle < 225.f) {
 			message.distances[i] = 700;
 
 		} else {
@@ -212,14 +396,15 @@ TEST_F(CollisionPreventionTest, outsideFOV)
 
 	for (int i = 0; i < distances_array_size; i++) {
 
-		float angle = math::radians((float)i * message.increment);
+		float angle_deg = (float)i * message.increment;
-		matrix::Vector2f original_setpoint = {10.f *(float)cos(angle), 10.f *(float)sin(angle)};
+		float angle_rad = math::radians(angle_deg);
+		matrix::Vector2f original_setpoint = {10.f *(float)cos(angle_rad), 10.f *(float)sin(angle_rad)};
 		matrix::Vector2f modified_setpoint = original_setpoint;
 		message.timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
 		cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
 
-		if (i > 0.25f * distances_array_size && i < 0.75f * distances_array_size) {
+		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);
@@ -234,7 +419,6 @@ TEST_F(CollisionPreventionTest, outsideFOV)
 	orb_unadvertise(obstacle_distance_pub);
 }
 
-
 TEST_F(CollisionPreventionTest, jerkLimit)
 {
 	// GIVEN: a simple setup condition
@@ -256,6 +440,7 @@ TEST_F(CollisionPreventionTest, jerkLimit)
 	message.min_distance = 100;
 	message.max_distance = 2000;
 	message.timestamp = hrt_absolute_time();
+	message.frame = message.MAV_FRAME_GLOBAL; //north aligned
 	int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
 	message.increment = 360 / distances_array_size;
 
@@ -283,3 +468,339 @@ TEST_F(CollisionPreventionTest, jerkLimit)
 	// THEN: the new setpoint should be much slower than the one with default jerk
 	EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm());
 }
+
+TEST_F(CollisionPreventionTest, addDistanceSensorData)
+{
+	// 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.current_distance = 5.f;
+
+	//THEN: at initialization the internal obstacle map should only contain UINT16_MAX
+	uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
+
+	for (uint32_t i = 0; i < distances_array_size; i++) {
+		EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+	}
+
+	//WHEN: we add distance sensor data to the right
+	distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING;
+	distance_sensor.h_fov = math::radians(19.99f);
+	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 == 8 || i == 9) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+	}
+
+	//WHEN: we add additionally distance sensor data to the left
+	distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING;
+	distance_sensor.h_fov = math::radians(50.f);
+	distance_sensor.current_distance = 8.f;
+	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 == 8 || i == 9) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else if (i >= 24 && i <= 29) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+	}
+
+	//WHEN: we add additionally distance sensor data to the front
+	distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
+	distance_sensor.h_fov = math::radians(10.1f);
+	distance_sensor.current_distance = 3.f;
+	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 == 8 || i == 9) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else if (i >= 24 && i <= 29) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800);
+
+		} else if (i == 0) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+	}
+
+
+}
+
+TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
+{
+	// 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 = 5.f;
+	obstacle_msg.min_distance = 20;
+	obstacle_msg.max_distance = 2000;
+	obstacle_msg.angle_offset = 0.f;
+
+	matrix::Quaternion<float> vehicle_attitude1(1, 0, 0, 0); //unit transform
+	matrix::Euler<float> attitude2_euler(0, 0, M_PI / 2.0);
+	matrix::Quaternion<float> vehicle_attitude2(attitude2_euler); //90 deg yaw
+	matrix::Euler<float> attitude3_euler(0, 0, -M_PI / 4.0);
+	matrix::Quaternion<float> vehicle_attitude3(attitude3_euler); // -45 deg yaw
+	matrix::Euler<float> attitude4_euler(0, 0, M_PI);
+	matrix::Quaternion<float> vehicle_attitude4(attitude4_euler); // 180 deg yaw
+
+	//obstacle at 10-30 deg world frame, distance 5 meters
+	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
+
+	for (int i = 2; i < 6 ; i++) {
+		obstacle_msg.distances[i] = 500;
+	}
+
+
+	//THEN: at initialization the internal obstacle map should only contain UINT16_MAX
+	int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
+
+	for (int i = 0; i < distances_array_size; i++) {
+		EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+	}
+
+	//WHEN: we add distance sensor data while vehicle has zero yaw
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i == 1 || i == 2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+
+	//WHEN: we add distance sensor data while vehicle yaw 90deg to the right
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i == 28 || i == 29) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we add distance sensor data while vehicle yaw 45deg to the left
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i == 6 || i == 7) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we add distance sensor data while vehicle yaw 180deg
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i == 19 || i == 20) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+}
+
+TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
+{
+	// 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_BODY_FRD; //north aligned
+	obstacle_msg.increment = 5.f;
+	obstacle_msg.min_distance = 20;
+	obstacle_msg.max_distance = 2000;
+	obstacle_msg.angle_offset = 0.f;
+
+	matrix::Quaternion<float> vehicle_attitude1(1, 0, 0, 0); //unit transform
+	matrix::Euler<float> attitude2_euler(0, 0, M_PI / 2.0);
+	matrix::Quaternion<float> vehicle_attitude2(attitude2_euler); //90 deg yaw
+	matrix::Euler<float> attitude3_euler(0, 0, -M_PI / 4.0);
+	matrix::Quaternion<float> vehicle_attitude3(attitude3_euler); // -45 deg yaw
+	matrix::Euler<float> attitude4_euler(0, 0, M_PI);
+	matrix::Quaternion<float> vehicle_attitude4(attitude4_euler); // 180 deg yaw
+
+	//obstacle at 10-30 deg body frame, distance 5 meters
+	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
+
+	for (int i = 2; i < 6 ; i++) {
+		obstacle_msg.distances[i] = 500;
+	}
+
+
+	//THEN: at initialization the internal obstacle map should only contain UINT16_MAX
+	int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
+
+	for (int i = 0; i < distances_array_size; i++) {
+		EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+	}
+
+	//WHEN: we add obstacle data while vehicle has zero yaw
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i == 1 || i == 2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we add obstacle data while vehicle yaw 90deg to the right
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i == 1 || i == 2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we add obstacle data while vehicle yaw 45deg to the left
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i == 1 || i == 2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we add obstacle data while vehicle yaw 180deg
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i == 1 || i == 2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+}
+
+
+TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset)
+{
+	// 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 = 6.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
+
+	//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 < 5 ; i++) {
+		obstacle_msg.distances[i] = 500;
+	}
+
+	//WHEN: we add distance sensor data
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude);
+
+	//THEN: the correct bins in the map should be filled
+	int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
+
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i >= 0 && i <= 2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we add distance sensor data with an angle offset
+	obstacle_msg.angle_offset = 30.f;
+	cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude);
+
+	//THEN: the correct bins in the map should be filled
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i >= 3 && i <= 5) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+}

src/lib/matrix

@@ -1 +1 @@
-Subproject commit cc084e0791535e8426780e6a4f05794804db1b82
+Subproject commit 22bf63cb714156eafd8a6d3822b903eba4980a8a

src/modules/mavlink/mavlink_receiver.cpp

@@ -1685,6 +1685,7 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
 	obstacle_distance.min_distance = mavlink_obstacle_distance.min_distance;
 	obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance;
 	obstacle_distance.angle_offset = mavlink_obstacle_distance.angle_offset;
+	obstacle_distance.frame = mavlink_obstacle_distance.frame;
 
 	_obstacle_distance_pub.publish(obstacle_distance);
 }