From e7d17cc2651b6dabf2a22d52313b22ba6df8cc4e Mon Sep 17 00:00:00 2001 From: baumanta Date: Thu, 16 May 2019 16:24:57 +0200 Subject: [PATCH] consider acceleration ramp-up time --- src/lib/CollisionPrevention/CollisionPrevention.cpp | 12 +++++++----- src/lib/CollisionPrevention/CollisionPrevention.hpp | 8 +++++--- .../ManualPosition/FlightTaskManualPosition.cpp | 2 +- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index acb4202c54..cfec3535fe 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -65,7 +65,7 @@ bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscriptio return true; } -void CollisionPrevention::calculate_constrained_setpoint(Vector2f &setpoint, const float max_acc) +void CollisionPrevention::calculate_constrained_setpoint(Vector2f &setpoint, const float max_acc, const float curr_vel) { const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get(); @@ -91,8 +91,10 @@ void CollisionPrevention::calculate_constrained_setpoint(Vector2f &setpoint, con float angle = math::radians((float)i * obstacle_distance.increment); //max admissible velocity in current bin direction: v = sqrt(2 * a * d), where d is the distance to standstill - //a is the constant acceleration and v the current velocity. We use a = a_max/2 to stay well within the limits - float vel_max_sqrd = math::max(0.f, max_acc * (distance - _param_mpc_col_prev_d.get())); + //a is the constant acceleration and v the current velocity. We use a = a_max/2 and j = j_max/2 to stay well within the limits + float acceleration_distance = 2.f * curr_vel * max_acc / _param_mpc_jerk_max.get(); + float distance_to_standstill = math::max(0.f, distance - _param_mpc_col_prev_d.get() - acceleration_distance); + float vel_max_sqrd = max_acc * distance_to_standstill; //split current setpoint into parallel and orthogonal components with respect to the current bin direction Vector2f bin_direction = {cos(angle), sin(angle)}; @@ -142,11 +144,11 @@ void CollisionPrevention::calculate_constrained_setpoint(Vector2f &setpoint, con } void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, - const float max_acc) + const float max_acc, const float curr_vel) { //calculate movement constraints based on range data Vector2f new_setpoint = original_setpoint; - calculate_constrained_setpoint(new_setpoint, max_acc); + calculate_constrained_setpoint(new_setpoint, max_acc, curr_vel); //warn user if collision prevention starts to interfere bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 42a2ddbde4..ea54165d4a 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -68,7 +68,8 @@ public: bool is_active() { return _param_mpc_col_prev_d.get() > 0; } - void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const float max_acc); + void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const float max_acc, + const float curr_vel); private: @@ -84,11 +85,12 @@ private: hrt_abstime _last_message; DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_col_prev_d /**< collision prevention keep minimum distance */ + (ParamFloat) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_mpc_jerk_max /**< maximum jerk allowed in position controller*/ ) void update(); - void calculate_constrained_setpoint(matrix::Vector2f &setpoint, const float max_acc); + void calculate_constrained_setpoint(matrix::Vector2f &setpoint, const float max_acc, const float curr_vel); }; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 49c5db61a8..91cdc1b316 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -134,7 +134,7 @@ void FlightTaskManualPosition::_scaleSticks() // collision prevention if (_collision_prevention.is_active()) { - _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, _param_mpc_acc_hor_max.get()); + _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, _param_mpc_acc_hor_max.get(), _velocity.norm()); } _velocity_setpoint(0) = vel_sp_xy(0);