Initial Working and cleaned up

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -508,19 +508,221 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 	}
 }
 
+void CollisionPrevention::_constrainAccelerationSetpoint(matrix::Vector2f &setpoint_accel,
+		const matrix::Vector2f &setpoint_vel)
+{
+	_updateObstacleMap();
+
+	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();
+	const float max_jerk = _param_mpc_jerk_max.get();
+	const float max_accel = _param_mpc_acc_hor.get();
+	const float xy_p = _param_mpc_xy_p.get();
+	const float vel_xy_p = _param_mpc_vel_p_acc.get(); // proportional gain for velocity
+	const float max_vel = _param_mpc_vel_manual.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_accel.norm();
+
+	const hrt_abstime constrain_time = getTime();
+	int num_fov_bins = 0;
+
+	float acc_vel_constraint = INFINITY;
+	matrix::Vector2f acc_vel_constraint_dir = {0.f, 0.f};
+	matrix::Vector2f acc_vel_constraint_setpoint = {0.f, 0.f};
+
+	if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
+
+		const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);
+		bool setpoint_possible = true;
+		matrix::Vector2f new_setpoint = {0.f, 0.f};
+
+		if (setpoint_length > 0.001f) {
+			Vector2f setpoint_dir = setpoint_accel / setpoint_length;
+
+			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);
+
+			float closest_distance = INFINITY;
+			matrix::Vector2f bin_closest_dist = {0.f, 0.f};
+
+
+			for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
+				// delete stale values
+				const hrt_abstime data_age = constrain_time - _data_timestamps[i];
+				const float max_range = _data_maxranges[i] * 0.01f;
+
+				if (data_age > RANGE_STREAM_TIMEOUT_US) {
+					_obstacle_map_body_frame.distances[i] = UINT16_MAX;
+				}
+
+				if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) {
+					num_fov_bins ++;
+				}
+
+				float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
+				angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
+
+				// get direction of current bin
+				const Vector2f bin_direction = {cosf(angle), sinf(angle)};
+
+				// only consider bins which are between min and max values
+				if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
+				    && _obstacle_map_body_frame.distances[i] < UINT16_MAX) {
+					const float distance = _obstacle_map_body_frame.distances[i] * 0.01f;
+
+					// Assume current velocity is sufficiently close to the setpoint velocity
+					const float curr_vel_parallel = math::max(0.f, setpoint_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 vel_max = math::min(vel_max_posctrl, vel_max_smooth);
+					const float acc_max_postrl = vel_xy_p * (vel_max - curr_vel_parallel) ;
+
+					if (acc_max_postrl < acc_vel_constraint) {
+						acc_vel_constraint = acc_max_postrl;
+						acc_vel_constraint_dir = bin_direction;
+					}
+
+					if (distance < closest_distance) {
+						closest_distance = distance;
+						bin_closest_dist = bin_direction;
+					}
+
+					// calculate closest distance for acceleration constraint
+
+
+				} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
+					if (!move_no_data || (move_no_data && _data_fov[i])) {
+						setpoint_possible = false;
+					}
+				}
+			}
+
+			const matrix::Vector2f normal_component = setpoint_dir * (setpoint_dir.dot(bin_closest_dist));
+			const matrix::Vector2f tangential_component = setpoint_dir - normal_component;
+
+
+			if (closest_distance < min_dist_to_keep && setpoint_possible) {
+				float scale = (closest_distance - min_dist_to_keep); // always negative meaning it will push us away from the obstacle
+				new_setpoint = tangential_component * setpoint_length  + bin_closest_dist * xy_p * vel_xy_p *
+					       scale; // scale is on the closest distance vector, as thats the critical direction
+
+			} else if (closest_distance >= min_dist_to_keep && setpoint_possible) {
+				const float scale_distance = math::max(min_dist_to_keep, max_vel / xy_p);
+				float scale = (closest_distance - min_dist_to_keep) / scale_distance;
+				scale *= scale; // square the scale to lower commanded accelerations close to the obstacle
+				scale = math::min(scale, 1.0f);
+
+
+				// only scale accelerations towards the obstacle
+				if (bin_closest_dist.dot(setpoint_dir) > 0) {
+					new_setpoint = (tangential_component + normal_component * scale) * setpoint_length;
+
+				} else {
+					new_setpoint = tangential_component * setpoint_length;
+				}
+
+			}
+
+			if (num_fov_bins == 0) {
+				new_setpoint.setZero();
+				PX4_WARN("No fov bins");
+			}
+
+			acc_vel_constraint_setpoint = acc_vel_constraint_dir * acc_vel_constraint;
+			setpoint_accel = new_setpoint + acc_vel_constraint_setpoint;
+
+		} else {
+			// If no setpoint is provided, still apply force when you are close to an obstacle
+			float closest_distance = INFINITY;
+			matrix::Vector2f bin_closest_dist = {0.f, 0.f};
+
+			for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
+				if (constrain_time - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) {
+					_obstacle_map_body_frame.distances[i] = UINT16_MAX;
+				}
+
+				//count number of bins in the field of valid_new
+				if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) {
+					num_fov_bins ++;
+				}
+
+				float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
+				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) {
+					const float distance = _obstacle_map_body_frame.distances[i] * 0.01f;
+
+					if (distance < closest_distance) {
+						closest_distance = distance;
+						bin_closest_dist = bin_direction;
+					}
+				}
+			}
+
+			if (closest_distance < min_dist_to_keep) {
+				float scale = (closest_distance - min_dist_to_keep);
+				new_setpoint = bin_closest_dist * xy_p * vel_xy_p * scale;
+			}
+
+			if (num_fov_bins == 0) {
+				new_setpoint.setZero();
+				PX4_WARN("No fov bins");
+			}
+			setpoint_accel = new_setpoint;
+		}
+
+	} else {
+		//allow no movement
+		PX4_WARN("No movement");
+		setpoint_accel.setZero();
+
+		// if distance data is stale, switch to Loiter
+		if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
+
+			if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US
+			    && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
+				_publishVehicleCmdDoLoiter();
+			}
+
+			_last_timeout_warning = getTime();
+		}
+	}
+
+}
 void
-CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
-				    const Vector2f &curr_vel)
+CollisionPrevention::modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel)
 {
+	const float max_accel = _param_mpc_acc_hor.get();
 	//calculate movement constraints based on range data
-	Vector2f new_setpoint = original_setpoint;
-	_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
+	matrix::Vector2f new_setpoint = setpoint_accel;
+	matrix::Vector2f original_setpoint = setpoint_accel;
+	_constrainAccelerationSetpoint(new_setpoint, setpoint_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);
+	bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_accel
+				      || new_setpoint(0) > original_setpoint(0) + 0.05f * max_accel
+				      || new_setpoint(1) < original_setpoint(1) - 0.05f * max_accel
+				      || new_setpoint(1) > original_setpoint(1) + 0.05f * max_accel);
 
 	_interfering = currently_interfering;
 
@@ -531,7 +733,7 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 	new_setpoint.copyTo(constraints.adapted_setpoint);
 	_constraints_pub.publish(constraints);
 
-	original_setpoint = new_setpoint;
+	setpoint_accel = new_setpoint;
 }
 
 void CollisionPrevention::_publishVehicleCmdDoLoiter()

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -79,9 +79,7 @@ class CollisionPrevention : public ModuleParams
 	 * @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);
-
+	void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
 protected:
 
 	obstacle_distance_s _obstacle_map_body_frame {};
@@ -106,6 +104,13 @@ class CollisionPrevention : public ModuleParams
 	 */
 	void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);
 
+	/**
+	 * Computes an adaption to the setpoint direction to guide towards free space
+	 * @param setpoint_accel current setpoint acceleration
+	 * @param setpoint_vel current setpoint velocity
+	 */
+	void _constrainAccelerationSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
+
 	/**
 	 * Determines whether a new sensor measurement is used
 	 * @param map_index, index of the bin in the internal map the measurement belongs in
@@ -148,7 +153,10 @@ class CollisionPrevention : public ModuleParams
 		(ParamBool<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*/
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/
+		(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode, /**< position control mode*/
+		(ParamFloat<px4::params::MPC_XY_VEL_P_ACC>) _param_mpc_vel_p_acc, /**< p gain from velocity controller*/
+		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual /**< p gain from velocity controller*/
 	)
 
 	/**

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -94,11 +94,6 @@ void FlightTaskManualPosition::_scaleSticks()
 	// Rotate setpoint into local frame
 	Sticks::rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint);
 
-	// collision prevention
-	if (_collision_prevention.is_active()) {
-		_collision_prevention.modifySetpoint(vel_sp_xy, velocity_scale, _position.xy(), _velocity.xy());
-	}
-
 	_velocity_setpoint.xy() = vel_sp_xy;
 }
 

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp

@@ -96,6 +96,13 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
 	Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
 	_acceleration_setpoint = stick_xy.emult(acceleration_scale);
 
+	if (_collision_prevention.is_active()) {
+		matrix::Vector2f accel_setpoint_xy = _acceleration_setpoint;
+		matrix::Vector2f vel_setpoint_xy = _velocity_setpoint;
+		_collision_prevention.modifySetpoint(accel_setpoint_xy, vel_setpoint_xy);
+		_acceleration_setpoint = accel_setpoint_xy;
+
+	}
 	// Add drag to limit speed and brake again
 	Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint);
 

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp

@@ -40,6 +40,7 @@
 #pragma once
 
 #include <px4_platform_common/module_params.h>
+#include <lib/collision_prevention/CollisionPrevention.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <matrix/math.hpp>
 #include <uORB/Subscription.hpp>
@@ -65,6 +66,8 @@ class StickAccelerationXY : public ModuleParams
 	void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); };
 
 private:
+	CollisionPrevention _collision_prevention{this};
+
 	void applyJerkLimit(const float dt);
 	matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy,
 				       const matrix::Vector2f &vel_sp);

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -93,6 +93,7 @@ if(gz-transport_FOUND)
 		windy
 		baylands
   		lawn
+		walls
 	)
 
 	# find corresponding airframes