From 232008854154758ab168602af7e8e18a4c92bbbf Mon Sep 17 00:00:00 2001 From: baumanta Date: Mon, 27 May 2019 17:21:17 +0200 Subject: [PATCH] use position controller parameters for limitation instead of acceleration/jerk --- .../CollisionPrevention.cpp | 22 +++++++++---------- .../CollisionPrevention.hpp | 10 +++++---- .../collisionprevention_params.c | 12 ++++++++++ .../FlightTaskManualPosition.cpp | 3 ++- 4 files changed, 30 insertions(+), 17 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 83ef4fb5b8..efb9e97822 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -88,8 +88,8 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se } } -void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const float max_acc, - const Vector2f &curr_vel) +void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, + const Vector2f &curr_pos, const Vector2f &curr_vel) { const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get(); @@ -99,7 +99,7 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f setpoint_sqrd = setpoint * setpoint_length; //Limit the deviation of the adapted setpoint from the originally given joystick input (slightly less than 90 degrees) - float max_slide_angle_rad = 1.2f; + float max_slide_angle_rad = 0.5f; if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { if (setpoint_length > 0.001f) { @@ -121,13 +121,11 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const float sp_orth = setpoint_sqrd.dot(orth_direction); float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction)); - //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 and j = j_max/2 to stay well within the limits - float acceleration_distance = 2.f * curr_vel_parallel * 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; - - + //calculate max allowed velocity with a P-controller (same gain as in the position controller) + float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get(); + float vel_max_posctrl = math::max(0.f, + _param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance)); + float vel_max_sqrd = vel_max_posctrl * vel_max_posctrl; //limit the setpoint to respect vel_max by subtracting from the parallel component if (sp_parallel > vel_max_sqrd) { @@ -171,11 +169,11 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const } void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, - const float max_acc, const Vector2f &curr_vel) + const Vector2f &curr_pos, const Vector2f &curr_vel) { //calculate movement constraints based on range data Vector2f new_setpoint = original_setpoint; - calculateConstrainedSetpoint(new_setpoint, max_acc, curr_vel); + calculateConstrainedSetpoint(new_setpoint, curr_pos, 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 aff3faf157..4c9e5a3711 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -68,8 +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, - const matrix::Vector2f &curr_vel); + void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, + const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); private: @@ -87,12 +87,14 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */ - (ParamFloat) _param_mpc_jerk_max /**< maximum jerk allowed in position controller*/ + (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ + (ParamFloat) _param_mpc_col_prev_dly /**< delay of the range measurement data*/ ) void update(); - void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const float max_acc, const matrix::Vector2f &curr_vel); + void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, + const matrix::Vector2f &curr_vel); void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index 030f943a65..a69a9ccc95 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -50,3 +50,15 @@ * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f); + +/** + * Average delay of the range sensor message in seconds + * + * Only used in Position mode. + * + * @min 0 + * @max 1 + * @unit seconds + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f); diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 039617130d..8437a6355f 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -134,7 +134,8 @@ void FlightTaskManualPosition::_scaleSticks() // collision prevention if (_collision_prevention.is_active()) { - _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, _param_mpc_acc_hor_max.get(), Vector2f(_velocity)); + _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, Vector2f(_position), + Vector2f(_velocity)); } _velocity_setpoint(0) = vel_sp_xy(0);