Add collision prevention velocity limitations also based on max accel…

…/jerk
PX4 · Aug 28, 2019 · 7af829f · 7af829f
1 parent e50dd7c
commit 7af829f
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 3 deletions.
diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/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

diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/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,
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
 	)
 
 	/**