From 09e1d4888cc27665eea1e5e2c27c9864049717a8 Mon Sep 17 00:00:00 2001 From: baumanta Date: Fri, 17 May 2019 09:00:19 +0200 Subject: [PATCH] use velocity component in bin direction instead of norm --- .../CollisionPrevention.cpp | 20 +++++++++++-------- .../CollisionPrevention.hpp | 4 ++-- .../FlightTaskManualPosition.cpp | 2 +- 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 140e7990ec..dca2ec47e9 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -65,7 +65,8 @@ bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscriptio return true; } -void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const float max_acc, const float curr_vel) +void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const float max_acc, + const Vector2f &curr_vel) { const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get(); @@ -90,17 +91,20 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters 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 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)}; Vector2f orth_direction = {-bin_direction(1), bin_direction(0)}; float sp_parallel = setpoint_sqrd.dot(bin_direction); 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; + + //limit the setpoint to respect vel_max by subtracting from the parallel component if (sp_parallel > vel_max_sqrd) { @@ -144,7 +148,7 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const } void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, - const float max_acc, const float curr_vel) + const float max_acc, const Vector2f &curr_vel) { //calculate movement constraints based on range data Vector2f new_setpoint = original_setpoint; diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 4925b40587..c1d99b92ca 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -69,7 +69,7 @@ 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 float curr_vel); + const matrix::Vector2f &curr_vel); private: @@ -91,6 +91,6 @@ private: void update(); - void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const float max_acc, const float curr_vel); + void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const float max_acc, const matrix::Vector2f &curr_vel); }; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 91cdc1b316..039617130d 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(), _velocity.norm()); + _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, _param_mpc_acc_hor_max.get(), Vector2f(_velocity)); } _velocity_setpoint(0) = vel_sp_xy(0);