CollisionPrevention: rewrite for Acceleration based manual flight mode

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,14 @@ 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 @@ public:
 	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

@@ -90,6 +90,7 @@ if(gz-transport_FOUND)
   		lawn
 		aruco
 		rover
+		walls
 	)
 
 	# find corresponding airframes