Collision prevention velocity limitations also based on max accel/jerk

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -38,6 +38,9 @@
  */
 
 #include <CollisionPrevention/CollisionPrevention.hpp>
+
+#include <FlightTasks/tasks/Utility/TrajMath.hpp>
+
 using namespace matrix;
 using namespace time_literals;
 
@@ -209,6 +212,8 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 	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();
 
 	if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
 		if (setpoint_length > 0.001f) {
@@ -240,8 +245,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 							//calculate max allowed velocity with a P-controller (same gain as in the position controller)
 							float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
 							float delay_distance = curr_vel_parallel * col_prev_dly;
-							float vel_max_posctrl = math::max(0.f, xy_p * (distance - min_dist_to_keep - delay_distance));
-							Vector2f  vel_max_vec = bin_direction * vel_max_posctrl;
+							float stop_distance =  math::max(0.f, distance - min_dist_to_keep - delay_distance);
+							float vel_max_posctrl = xy_p * stop_distance;
+							float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
+							Vector2f  vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth);
 							float vel_max_bin = vel_max_vec.dot(setpoint_dir);
 
 							//constrain the velocity

src/lib/CollisionPrevention/CollisionPrevention.hpp

@@ -96,7 +96,9 @@ class CollisionPrevention : public ModuleParams
 		(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */
 		(ParamFloat<px4::params::MPC_COL_PREV_ANG>) _param_mpc_col_prev_ang, /**< collision prevention detection angle */
 		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
-		(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly /**< delay of the range measurement data*/
+		(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly, /**< delay of the range measurement data*/
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
 	)
 
 	/**

src/lib/CollisionPrevention/CollisionPreventionTest.cpp

@@ -233,3 +233,53 @@ TEST_F(CollisionPreventionTest, outsideFOV)
 
 	orb_unadvertise(obstacle_distance_pub);
 }
+
+
+TEST_F(CollisionPreventionTest, jerkLimit)
+{
+	// GIVEN: a simple setup condition
+	TestCollisionPrevention cp;
+	matrix::Vector2f original_setpoint(10, 0);
+	float max_speed = 3;
+	matrix::Vector2f curr_pos(0, 0);
+	matrix::Vector2f curr_vel(2, 0);
+
+	// AND: distance set to 5m
+	param_t param = param_handle(px4::params::MPC_COL_PREV_D);
+	float value = 5; // try to keep 5m distance
+	param_set(param, &value);
+	cp.paramsChanged();
+
+	// AND: an obstacle message
+	obstacle_distance_s message;
+	memset(&message, 0xDEAD, sizeof(message));
+	message.min_distance = 100;
+	message.max_distance = 2000;
+	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++) {
+		message.distances[i] = 700;
+	}
+
+	// AND: 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_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
+	matrix::Vector2f modified_setpoint_default_jerk = original_setpoint;
+	cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel);
+	orb_unadvertise(obstacle_distance_pub);
+
+	// AND: we now set max jerk to 0.1
+	param = param_handle(px4::params::MPC_JERK_MAX);
+	value = 0.1; // 0.1 maximum jerk
+	param_set(param, &value);
+	cp.paramsChanged();
+
+	// WHEN: we run the setpoint modification again
+	matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint;
+	cp.modifySetpoint(modified_setpoint_limited_jerk, max_speed, curr_pos, curr_vel);
+
+	// 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());
+}