lib: name folders consistently

src/lib/collision_prevention/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(CollisionPrevention CollisionPrevention.cpp)
+target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable
+
+px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention )

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -0,0 +1,527 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file CollisionPrevention.cpp
+ * CollisionPrevention controller.
+ *
+ */
+
+#include "CollisionPrevention.hpp"
+
+using namespace matrix;
+using namespace time_literals;
+
+namespace
+{
+static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly
+static constexpr int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG;
+
+static float wrap_360(float f)
+{
+	return wrap(f, 0.f, 360.f);
+}
+
+static int wrap_bin(int i)
+{
+	i = i % INTERNAL_MAP_USED_BINS;
+
+	while (i < 0) {
+		i += INTERNAL_MAP_USED_BINS;
+	}
+
+	return i;
+}
+
+} // namespace
+
+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;
+		_data_maxranges[i] = 0;
+		_obstacle_map_body_frame.distances[i] = UINT16_MAX;
+	}
+}
+
+CollisionPrevention::~CollisionPrevention()
+{
+	//unadvertise publishers
+	if (_mavlink_log_pub != nullptr) {
+		orb_unadvertise(_mavlink_log_pub);
+	}
+}
+
+hrt_abstime CollisionPrevention::getTime()
+{
+	return hrt_absolute_time();
+}
+
+hrt_abstime CollisionPrevention::getElapsedTime(const hrt_abstime *ptr)
+{
+	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) {
+				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;
+				}
+			}
+		}
+
+	} 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) {
+
+				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;
+				}
+			}
+		}
+
+	} else {
+		mavlink_log_critical(&_mavlink_log_pub, "Obstacle message received in unsupported frame %.0f\n",
+				     (double)obstacle.frame);
+	}
+}
+
+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();
+
+	// 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 instances with valid data and orientations useful for collision prevention
+			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
+				_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
+				_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
+									(uint16_t)(distance_sensor.max_distance * 100.0f));
+				_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
+									(uint16_t)(distance_sensor.min_distance * 100.0f));
+
+				_addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q));
+			}
+		}
+	}
+
+	// add obstacle distance data
+	if (_sub_obstacle_distance.update()) {
+		const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
+
+		// 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));
+		}
+	}
+
+	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
+	_obstacle_distance_pub.publish(_obstacle_map_body_frame);
+}
+
+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_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) /
+					     INTERNAL_MAP_INCREMENT_DEG);
+		int upper_bound = (int)floor((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) /
+					     INTERNAL_MAP_INCREMENT_DEG);
+
+		// floor values above zero, ceil values below zero
+		if (lower_bound < 0) { lower_bound++; }
+
+		if (upper_bound < 0) { upper_bound++; }
+
+		// rotate vehicle attitude into the sensor body frame
+		matrix::Quatf attitude_sensor_frame = vehicle_attitude;
+		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);
+
+			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)
+{
+	const float col_prev_d = _param_cp_dist.get();
+	const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG);
+	const int sp_index_original = setpoint_index;
+	float best_cost = 9999.f;
+	int new_sp_index = setpoint_index;
+
+	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
+		const 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];
+			}
+		}
+
+		const int bin = wrap_bin(i);
+		mean_dist = mean_dist / (2.f * filter_size + 1.f);
+		const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original);
+		const 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;
+			new_sp_index = bin;
+		}
+	}
+
+	//only change setpoint direction if it was moved to a different bin
+	if (new_sp_index != setpoint_index) {
+		float angle = math::radians((float)new_sp_index * 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 = new_sp_index;
+	}
+}
+
+float
+CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
+{
+	float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f;
+
+	switch (distance_sensor.orientation) {
+	case distance_sensor_s::ROTATION_YAW_0:
+		offset = 0.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_45:
+		offset = M_PI_F / 4.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_90:
+		offset = M_PI_F / 2.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_135:
+		offset = 3.0f * M_PI_F / 4.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_180:
+		offset = M_PI_F;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_225:
+		offset = -3.0f * M_PI_F / 4.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_270:
+		offset = -M_PI_F / 2.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_315:
+		offset = -M_PI_F / 4.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_CUSTOM:
+		offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
+		break;
+	}
+
+	return offset;
+}
+
+void
+CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
+		const Vector2f &curr_vel)
+{
+	_updateObstacleMap();
+
+	// read parameters
+	const float col_prev_d = _param_cp_dist.get();
+	const float col_prev_dly = _param_cp_delay.get();
+	const bool move_no_data = _param_cp_go_nodata.get() > 0;
+	const float xy_p = _param_mpc_xy_p.get();
+	const float max_jerk = _param_mpc_jerk_max.get();
+	const float max_accel = _param_mpc_acc_hor.get();
+	const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
+	const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
+
+	const float setpoint_length = setpoint.norm();
+
+	const hrt_abstime constrain_time = getTime();
+
+	if ((constrain_time - _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;
+			const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);
+
+			const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
+			const 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_cp_guide_ang 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
+				const hrt_abstime data_age = constrain_time - _data_timestamps[i];
+
+				if (data_age > RANGE_STREAM_TIMEOUT_US) {
+					_obstacle_map_body_frame.distances[i] = UINT16_MAX;
+				}
+
+				const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters
+				const 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]
+				angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
+
+				// get direction of current bin
+				const Vector2f bin_direction = {cosf(angle), sinf(angle)};
+
+				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) {
+						// calculate max allowed velocity with a P-controller (same gain as in the position controller)
+						const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
+						float delay_distance = curr_vel_parallel * col_prev_dly;
+
+						if (distance < max_range) {
+							delay_distance += curr_vel_parallel * (data_age * 1e-6f);
+						}
+
+						const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
+						const float vel_max_posctrl = xy_p * stop_distance;
+
+						const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
+						const 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) {
+							vel_max = math::min(vel_max, vel_max_bin);
+						}
+					}
+
+				} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index && (!move_no_data)) {
+					vel_max = 0.f;
+				}
+			}
+
+			setpoint = setpoint_dir * vel_max;
+		}
+
+	} else {
+		// if distance data is stale, switch to Loiter
+		_publishVehicleCmdDoLoiter();
+		mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
+	}
+}
+
+void
+CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
+				    const Vector2f &curr_vel)
+{
+	//calculate movement constraints based on range data
+	Vector2f new_setpoint = original_setpoint;
+	_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
+
+	//warn user if collision prevention starts to interfere
+	bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
+				      || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
+				      || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
+				      || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);
+
+	if (currently_interfering && !_interfering) {
+		if (hrt_elapsed_time(&_last_collision_warning) > 3_s) {
+			mavlink_log_critical(&_mavlink_log_pub, "Collision Warning");
+			_last_collision_warning = hrt_absolute_time();
+		}
+	}
+
+	_interfering = currently_interfering;
+
+	// publish constraints
+	collision_constraints_s	constraints{};
+	constraints.timestamp = hrt_absolute_time();
+	original_setpoint.copyTo(constraints.original_setpoint);
+	new_setpoint.copyTo(constraints.adapted_setpoint);
+	_constraints_pub.publish(constraints);
+
+	original_setpoint = new_setpoint;
+}
+
+void CollisionPrevention::_publishVehicleCmdDoLoiter()
+{
+	vehicle_command_s command{};
+	command.timestamp = hrt_absolute_time();
+	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
+	command.param1 = (float)1; // base mode
+	command.param3 = (float)0; // sub mode
+	command.target_system = 1;
+	command.target_component = 1;
+	command.source_system = 1;
+	command.source_component = 1;
+	command.confirmation = false;
+	command.from_external = false;
+	command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
+	command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+
+	// publish the vehicle command
+	_vehicle_command_pub.publish(command);
+}

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file CollisionPrevention.hpp
+ * @author Tanja Baumann <tanja@auterion.com>
+ *
+ * CollisionPrevention controller.
+ *
+ */
+
+#pragma once
+
+#include <float.h>
+
+#include <commander/px4_custom_mode.h>
+#include <drivers/drv_hrt.h>
+#include <mathlib/mathlib.h>
+#include <matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/collision_constraints.h>
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_command.h>
+
+using namespace time_literals;
+
+class CollisionPrevention : public ModuleParams
+{
+public:
+	CollisionPrevention(ModuleParams *parent);
+
+	virtual ~CollisionPrevention();
+	/**
+	 * Returs true if Collision Prevention is running
+	 */
+	bool is_active() { return _param_cp_dist.get() > 0; }
+
+	/**
+	 * Computes collision free setpoints
+	 * @param original_setpoint, setpoint before collision prevention intervention
+	 * @param max_speed, maximum xy speed
+	 * @param curr_pos, current vehicle position
+	 * @param curr_vel, current vehicle velocity
+	 */
+	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])];
+	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 &vehicle_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);
+
+	/**
+	 * 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);
+
+
+private:
+
+	bool _interfering{false};		/**< states if the collision prevention interferes with the user input */
+
+	orb_advert_t _mavlink_log_pub{nullptr};	 	/**< Mavlink log uORB handle */
+
+	uORB::Publication<collision_constraints_s>	_constraints_pub{ORB_ID(collision_constraints)};		/**< constraints publication */
+	uORB::Publication<obstacle_distance_s>		_obstacle_distance_pub{ORB_ID(obstacle_distance_fused)};	/**< obstacle_distance publication */
+	uORB::PublicationQueued<vehicle_command_s>	_vehicle_command_pub{ORB_ID(vehicle_command)};			/**< vehicle command do publication */
+
+	uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
+	uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
+	uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
+
+	static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
+
+	hrt_abstime	_last_collision_warning{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
+		(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
+		(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
+		(ParamFloat<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
+		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
+	)
+
+	/**
+	 * Transforms the sensor orientation into a yaw in the local frame
+	 * @param distance_sensor, distance sensor message
+	 * @param angle_offset, sensor body frame offset
+	 */
+	float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const;
+
+	/**
+	 * Computes collision free setpoints
+	 * @param setpoint, setpoint before collision prevention intervention
+	 * @param curr_pos, current vehicle position
+	 * @param curr_vel, current vehicle velocity
+	 */
+	void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos,
+					   const matrix::Vector2f &curr_vel);
+
+	/**
+	 * Publishes collision_constraints message
+	 * @param original_setpoint, setpoint before collision prevention intervention
+	 * @param adapted_setpoint, collision prevention adaped setpoint
+	 */
+	void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
+
+	/**
+	 * Publishes obstacle_distance message with fused data from offboard and from distance sensors
+	 * @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 _updateObstacleMap();
+
+	/**
+	 * Publishes vehicle command.
+	 */
+	void _publishVehicleCmdDoLoiter();
+
+};

src/lib/collision_prevention/collisionprevention_params.c

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file collisionprevention_params.c
+ *
+ * Parameters defined by the collisionprevention lib.
+ *
+ * @author Tanja Baumann <tanja@auterion.com>
+ */
+
+/**
+ * Minimum distance the vehicle should keep to all obstacles
+ *
+ * Only used in Position mode. Collision avoidance is disabled by setting this parameter to a negative value
+ *
+ * @min -1
+ * @max 15
+ * @unit meters
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(CP_DIST, -1.0f);
+
+/**
+ * Average delay of the range sensor message plus the tracking delay of the position controller in seconds
+ *
+ * Only used in Position mode.
+ *
+ * @min 0
+ * @max 1
+ * @unit seconds
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(CP_DELAY, 0.4f);
+
+/**
+ * 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.
+ *
+ * @min 0
+ * @max 90
+ * @unit [deg]
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(CP_GUIDE_ANG, 30.f);
+
+/**
+ * Boolean to allow moving into directions where there is no sensor data (outside FOV)
+ *
+ * Only used in Position mode.
+ *
+ * @boolean
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(CP_GO_NO_DATA, 0);