From 6535d5123e29143fc68f98e8754268c503672d6d Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 25 Oct 2019 11:08:04 +0200 Subject: [PATCH] AutoLineSmoothVel: fix constrain priority for autocontinue. The constrainAbs function was not prioritizing the minimum value that produces the autocontinue behaviour. This caused zig-zag paths when the waypoints were almost -but not exactly- aligned. --- .../FlightTaskAutoLineSmoothVel.cpp | 14 +++++++------- .../FlightTaskAutoLineSmoothVel.hpp | 18 ++++++++++++++---- 2 files changed, 21 insertions(+), 11 deletions(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 45a1b9b633..25cbd27fb0 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -168,7 +168,7 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj() * Example: - if the constrain is -5, the value will be constrained between -5 and 0 * - if the constrain is 5, the value will be constrained between 0 and 5 */ -inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint) +float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint) { const float min = (constraint < FLT_EPSILON) ? constraint : 0.f; const float max = (constraint > FLT_EPSILON) ? constraint : 0.f; @@ -176,12 +176,12 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con return math::constrain(val, min, max); } -float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float min, float max) +float FlightTaskAutoLineSmoothVel::_constrainAbsPrioritizeMin(float val, float min, float max) { - return math::sign(val) * math::constrain(fabsf(val), fabsf(min), fabsf(max)); + return math::sign(val) * math::max(math::min(fabsf(val), fabsf(max)), fabsf(min)); } -float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() +float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const { // Compute the maximum allowed speed at the waypoint assuming that we want to // connect the two lines (prev-current and current-next) @@ -217,7 +217,7 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() return speed_at_target; } -float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) +float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) const { float max_speed = math::trajectory::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(), _param_mpc_acc_hor.get(), @@ -277,8 +277,8 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() // Constrain the norm of each component using min and max values Vector2f vel_sp_constrained_xy; - vel_sp_constrained_xy(0) = _constrainAbs(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0)); - vel_sp_constrained_xy(1) = _constrainAbs(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1)); + vel_sp_constrained_xy(0) = _constrainAbsPrioritizeMin(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0)); + vel_sp_constrained_xy(1) = _constrainAbsPrioritizeMin(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1)); for (int i = 0; i < 2; i++) { // If available, constrain the velocity using _velocity_setpoint(.) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index cee583b23c..899f14393b 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -77,11 +77,21 @@ protected: void _generateHeading(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ - inline float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */ - inline float _constrainAbs(float val, float min, float max); /**< Constrain absolute value of val between min and max */ + static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */ - float _getSpeedAtTarget(); - float _getMaxSpeedFromDistance(float braking_distance); + /** + * Constrain the abs value below max but above min + * Min can be larger than max and has priority over it + * The whole computation is done on the absolute values but the returned + * value has the sign of val + * @param val the value to constrain and boost + * @param min the minimum value that the function should return + * @param max the value by which val is constrained before the boost is applied + */ + static float _constrainAbsPrioritizeMin(float val, float min, float max); + + float _getSpeedAtTarget() const; + float _getMaxSpeedFromDistance(float braking_distance) const; void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */ void _updateTrajConstraints();