CollisionPrevention: Added Case where velocity gets reduced to zero i…

…f we are closer to the obstacle than the minimal distance

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -600,12 +600,18 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic
 				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 stop_distance = distance - _min_dist_to_keep - delay_distance;
 
-			const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(),
-					      _param_mpc_acc_hor.get(), stop_distance, 0.f);
-			// we dont take the minimum of the last term because of stop_distance is zero but current velocity is not, we want the acceleration to become negative and slow us down.
-			const float curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * (max_vel - curr_vel_parallel);
+			float curr_acc_vel_constraint;
+
+			if (stop_distance >= 0.f) {
+				const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(),
+						      _param_mpc_acc_hor.get(), stop_distance, 0.f);
+				curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f);
+
+			} else {
+				curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel;
+			}
 
 			if (curr_acc_vel_constraint < vel_comp_accel) {
 				vel_comp_accel = curr_acc_vel_constraint;